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ABSTRACT 


We  present  an  algorithm  for  finding  optimal  three- 
dimensional  paths  above  polyhedral  models  of  terrain. 
Airspace  is  modeled  as  irregularly-shaped  regions  of 
homogeneous  probability-of-detection,  with  respect  to  one  or 
more  fixed  observers.  We  plan  paths  by  first  finding  an 
optimal  set  of  contiguous  visibility  regions,  then  an  optimal 
piecewise-linear  flight  path  through  this  envelope,  using 
Snell's  Law  to  find  locally-optimal  maneuver  points.  The 
performance  of  our  region-finding  algorithm  favorably 
compares  with  an  alternate  approach  using  regular  cubic 
regions . 
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I .  INTRODUCTION 


Path  planning  is  an  important  and  growing  area  of 
research  in  artificial  intelligence.  Recent  research  has 
produced  an  extensive  and  sophisticated  body  of  knowledge. 
Much  of  this  work  has  been  restricted  to  the  (essentially) 
two-dimensional  path  planning  problem  for  a  ground  vehicle 
moving  on  ground  terrain.  However,  many  interesting  and 
potentially  useful  path-planning  problems  involve  true  three- 
dimensional  paths.  Examples  include  aircraft  flight-paths, 
submersible  paths  underwater,  overhead-crane  paths  within 
large  factories,  tunneling  plans  for  underground  shaft  mines, 
and  routes  for  microwave  communications  links.  Many  of  these 
areas  have  potential  for  application  within  the  U.  S,  Navy. 

Unfortunately,  the  three-dimensional  path-planning 
problem  is  not  a  simple  extension  of  the  two-dimensional 
path-planning  problem.  Many  physical  restrictions  exist 
which  constrain  the  three-dimensional  problem  in  ways  which 
are  fundamentally  different  from  the  constraints  found  in 
two-dimensional  problems  (it  is  generally  not  possible  to 
stop  in  mid-air  while  flying,  for  instance).  Certain 
abstractions  may  be  made  to  simplify  the  problem.  If  one 
thinks  of  the  two-dimensional  path  planning  problem  as 
finding  a  path  through  a  flat,  two-dimensional  graph  of  a 
given  precision  (or  granularity),  then  a  three-dimensional 
path  planning  problem  can  be  thought  of  as  a  search  through  a 
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three-dimensional  graph,  again  of  a  given  granularity.  This 
graph  is  the  search  space. 

If  one  considers  a  cube  of  air  1000  meters  on  a  side  as  a 
search  space,  dividing  it  into  10  meter  cubes  will  produce  a 
search  graph  containing  one  million  nodes.  Using  25-meter 
cubes,  64  thousand  nodes  will  be  needed  to  define  the  search 
graph.  Using  50-meter  cubes,  eight  thousand  nodes  will  be 
needed  This  is  illustrated  in  Figure  1. 

To  handle  this  over-abundance  of  search  space,  certain 
assumptions  can  be  made  concerning  the  type  of  path  desired 
and  the  nature  of  the  search  graph.  One  method  is  to  reduce 
the  volume  of  the  search  space  by  aggressive  pruning  (e.g., 
making  assumptions)  as  the  search  progresses.  This  thesis 
proposes  an  alternate  method  for  reducing  the  size  of  the 
search  space,  where,  by  coalescing  nodes  sharing  some  common 
characteristic,  the  size  of  the  search  space  is  reduced 
before  the  search  starts.  Other  characteristics  (such  as 
travel  cost,  altitude,  cloudiness,  humidity,  for  example)  of 
the  original  nodes  must  then  be  compensated  for  while 
performing  the  actual  search.  To  illustrate  this  approach,  a 
subclass  of  the  generalized  three-dimensional  routing  problem 
was  selected  for  analysis.  Only  two  factors  were  used  in 
optimization  while  path  planning  through  the  three- 
dimensional  search  graph:  visibility  (probability-of- 
detection) ,  and  cost  (or  energy  expended).  Visibility  will 
be  used  as  the  grouping  characteristic,  and  cost  will  be  used 
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Figure  1.  Search  Graph  for  Simple  3-D  Search  in 
Cross-Section 
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as  the  coBpensating  characteristic  which  must  be  compensated 


for  as  the  search  progresses.  All  path  planning  will  be 
conducted  over  idealized,  polyhedral  approximations  of 
terrain. 

This  thesis  will  thus  consider  three-dimensional  routing 
problems  which  plan  routes  between  two  given  points  in  space 
over  terrain  modeled  as  a  polyhedron,  optimizing  the  route 
for  low  probability-of-detection  and  cost,  given  a  finite 
number  of  immobile  observers  to  generate  the  visibility 
criteria.  This  subclass  of  problems  includes  such  "real 
world"  examples  as  route  planning  for  cruise  missiles  from 
launch  point  to  final  approach  on  target;  routing  of  manned 
attack  aircraft  to  (and  from)  a  mission  objective;  the 
routing  of  a  submersible  through  a  field  of  active  sonar 
devices;  and  detecting  "holes"  in  the  area  air  defense  of  a 
high  value  target.  Optimizing  for  high  probability-of- 
detection  instead  of  low  probability-of-detection  has 
application  to  another  class  of  problems,  such  as  routing  of 
commercial  passenger  aircraft  between  radar  sites, 
maintaining  a  Remotely  Piloted  Vehicle  within  line-of-sight 
(LOS)  communication  of  one  or  more  controllers,  or  ensuring 
that  ground-based  autonomous  vehicles  remain  within  LOS 
communication  of  one  or  more  communication  sites  for  as  long 
as  possible. 

Given  a  piece  of  terrain,  and  an  observer,  as  in  Figure 
2,  the  airspace  can  be  divided  into  two  regions;  airspace 
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Visible  Region 


Figure  2.  The  Concept  of  Visibility  Regions 


I - 

I 

j 

that  is  visible  to  the  observer,  and  airspace  which  is  not. 
These  regions  can  be  determined  by  a  simple  construction 
involving  the  observer  and  the  terrain.  All  of  the  air 
within  a  given  visibility  region  will  be  considered  as  a 
single  entity  when  discussing  visibility  of  that  region;  any 
one  point  within  the  region  is  no  more  or  less  visible  than 
any  other  point.  Using  this  argument,  then,  the  airspace 
above  the  observer  in  Figure  3  can  be  reduced  to  a  very 
simple  graph  when  evaluating  visibility.  In  terms  of  Figure 
3b  and  3c,  being  "not  visible"  means  being  at  graph  node  3; 
being  "visible"  means  being  at  graph  node  1  or  2.  This 
abstraction  of  the  visibility  of  space  will  be  a  powerful 
tool  in  the  three-dimensional  path  planning  which  follows. 
Figure  4  illustrates  a  more  complex  example. 

The  second  aspect  of  the  class  of  path  planning 
considered  here  involves  minimizing  the  energy  expended  to 
get  between  two  points.  Energy  costs  are  a  function  of 
location  ard  velocity.  These  costs  depend  upon  the  scale  of 
the  terrain,  and  the  type  of  device  which  is  expected  to 
follow  the  final  path.  To  simplify  matters,  a  more  "generic" 
approximation  based  on  distance  travelled  was  developed, 
using  linear  corrections  to  account  for  energy  lost  or  gained 
while  turning,  diving  and  climbing. 

Path  analysis  for  the  problems  discussed  above  involves 
combining  visibility  information  obtained  from  a  visibility 
model  with  energy  costs  in  an  appropriate  manner.  An  optimal 
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Visible  (Region  1) 


(a) 
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Search  Graph  with  Spatial  Relationships 
(b) 


1 

2 
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Abstract  Search  Graph 

(c) 


Figure  3.  Construction  of  a  Search  Graph 
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path,  or  "best  path,"  minimizes  the  weighted  average  of  the 
probability-of-detection  and  energy  that  must  be  expended. 
In  general,  this  optimal  path  will  not  be  the  same  as  either 
the  minimum  energy  path  (ignoring  visibility),  or  the  minimum 
probability-of-detection  path  (ignoring  cost). 

An  artificial-intelligence  search  method  called  A*  search 
can  perform  the  visibility-region  sequence-planning  within 
the  context  of  the  visibility  and  cost  modelling  techniques 
discussed  above.  However,  the  path-planning  problem  is  not 
then  completely  solved,  since  A*  search  will  produce  a 
contiguous  series  of  regions  (volumes)  with  common  visibility 
characteristics  shown  in  Figure  5,  called  a  volume  path.  An 
optimal  piecewise  linear  path  must  still  be  found  within  the 
confines  of  the  volume  path  produced  by  the  A*  search,  such 
that  the  path  is  linear  within  each  volume.  Determination  of 
the  optimal  piecewise-linear  flight  path  through  the  volume 
path  reduces  to  an  optimization  problem  in  three-dimensions. 
Fermat's  Principle  from  optics  can  be  used  to  solve  this 
optimization  problem. 

The  remaining  sections  of  this  thesis  describe  an 
implementation  of  the  three-dimensional  path  planning 
approach  discussed  above.  Chapter  II  introduces  all  relevant 
background  topics,  including  solid  geometry.  A*  search  and 
Fermat’s  Principle.  Chapter  III  describes  the  visibility¬ 
grouping  and  path-planning  algorithms  in  detail,  while 
Chapter  IV  describes  the  details  of  their  algorithms. 
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(a)  Volume  Path 

®  Start  Point 


'  (b)  Piecewise-iinear  Path 

Start  Point 


Figure  5.  Types  of  Paths 
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Results  of  the  implementation  will  be  presented  in  Chapter  V, 
which  shows  both  the  visibility  models  and  some 
representative  flight  paths  over  a  variety  of  terrain 
features.  Finally,  Chapter  VI  discusses  the  strengths  and 
weaknesses  found  in  the  algorithms  and  their  Implementations. 


II.  BACKGROUND 


A  significant  amount  of  previous  work  has  been  done  in 
path-planning,  although  very  little  of  it  has  involved  three- 
dimensional  path-planning  [Ref.  1].  A  family  of  generalized 
search  algorithms  has  been  developed  [Ref  2]  which  can  be 
applied  to  almost  any  sort  of  search  problem  when  the  search 
nodes  are  discrete  and  ennumerable.  These  generalized  search 
methods  have  been  successfully  applied  to  various  two- 
dimensional  path-planning  problems  [Ref.  3],  [Ref.  4]  and 
[Ref.  5],  and  two  of  these  generalized  search  methods  will  be 
used  in  the  three-dimensional  path-planning  problem  presented 
in  this  thesis.  Only  a  small  body  of  published  literature 
discusses  three-dimensional  path-planning,  and  that  addresses 
rather  abstract  versions  of  the  problem  (for  example,  [Ref. 
6]  discusses  path-planning  along  the  surface  of  a  solid 
convex  volume) . 

A.  RELATED  PROBLEMS  AND  SOLUTIONS 

1.  General  Predefined-Node  Search  Algorithms 

[Ref.  2]  contains  a  description  of  all  generalized 
search  algorithms.  Table  9-5  from  that  reference  is 
reproduced  as  Table  1,  illustrating  most  of  the  currently 
used  generalized  search  algorithms,  and  some  of  their 
features.  All  of  these  search  functions  seek  to  find  some 
path  (often  an  optimal  path,  but  not  always)  between  a  given 
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TABLE  1 .  SEARCH  METHODS 


Name  of 
search  strategy 

i 

Uses 

agenda? 

Uses 

evaluation 

function? 

Uses 

cost 

function? 

Next  state  whose 
successors  are  found 

Depth-first 

search 

no 

no 

no 

A  successor  of  the  last 
state,  else  a  successor 
of  a  predecessor 

Breadth-lirst 

search 

yes 

no 

no 

The  state  on  the 
agenda  the  longest 

Hill-climbing 

(optimization) 

no 

yes 

no 

The  lowest-evaluation 
successor  of  the  last  state 

Best-first 

search 

yes 

yes 

no 

The  state  on  the  agenda 
of  lowest  evaluation  value 

Branch-and-bound 

yes 

no 

yes 

The  state  on  the  agenda 
of  lowest  total  cost 

A*  search 

yes 

yes 

yes 

The  state  on  the  agenda 
of  lowest  sum  of  evaluation 
value  and  total  cost 

n 


Reprinted  from  Rowe,  Neil  C.,  Artificial  Intelligence 
Through  Prolog,  Prentice-Hall,  1988 


start  point  to  a  given  goal  point,  through  a  finite  search 
space.  The  search  space  may  be  a  representation  of  physical 
objects,  such  as  a  graph  representing  terrain,  or  it  may 
represent  more  abstract  concepts,  such  as  the  set  of  all 
possible  steps  in  fixing  a  car  [Ref.  2].  Most  searches  that 
find  optimal  paths  (A*,  Best-first,  Branch-and-bound) ,  use 
some  sort  of  metric  to  judge  the  distance  to  either  the  start 
point,  goal  point  or  both.  These  metrics  are  then  used  to 
decide  which  paths  through  the  search  space  are  optimal,  or 
lowest  cost.  These  metrics  are  called  cost  functions  and 
evaluation  functions. 

A  cost  function  is  a  function  which  produces  a  number 
representing  the  effort  (or  cost,  or  energy  expended)  to  get 
to  a  given  point  in  the  search  space  from  the  start  point. 
An  evaluation  function  is  similar  to  a  cost  function,  except 
that  it  produces  a  number  which  indicates  the  expected  cost 
to  get  to  the  goal  from  a  given  location  in  the  search  space. 
A*  search  uses  the  sum  of  the  cost  and  evaluation  functions 
to  decide  which  paths  are  optimal  (e.g.,  lowest  total  cost) 
as  the  search  progresses  through  the  search  space  towards  the 
goal.  Because  of  this.  A*  search  is  generally  regarded  as 
the  best  of  the  search  methods,  even  though  it  is  the  most 
complicated  [Ref.  2].  Other  search  methods  can  be  used  if 
optimality  can  be  sacrificed,  or  if  the  particular  problem  at 
hand  can  be  best  solved  using  a  less  sophisticated  search 
algorithm. 
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The  three-dimensional  path  planning  program  uses  fii* 
search  for  path-planning  through  the  visibility  volumes,  and 
the  best-first  search  in  one  application  where  the  cost 
function  has  no  meaning. 

2.  Path  Planning  in  Two-Dimensions 

Current  work  in  two-dimensional  path  planning  can  be 
divided  into  three  major  approaches;  path-oriented 
approaches,  spatial-reasoning  based  algorithms  and  blind 
search  methods  [Ref.  7].  The  path-oriented  approach  uses  a 
search  space  of  all  possible  paths  (developed  by  some  other 
search  method)  from  the  start  to  the  goal,  and  then  selects 
the  best  path  from  this  list  using  concepts  from  decision 
theory  [Ref.  2].  The  spatial  reasoning  approach  reasons 
about  how  paths  are  affected  by  obstacles.  An  example  of 
this  is  the  visibility  graph.  Visibility  graphs,  or  V- 
graphs,  have  a  search  space  composed  of  lines  of  mutual 
visibility  between  edges  and  vertices  of  obstacles.  Path 
planning  is  accomplished  along  these  lines,  from  the  start 
point  to  the  goal.  Blind  searches  generally  use  a  form  of 
wavefront  propagation  search,  which  subdivides  the  search 
area  into  small  squares,  and  then  finds  paths  by  looking,  one 
square  at  a  time,  in  all  directions,  until  the  goal  is 
reached.  The  search  graph  in  this  search  consists  of  the 
squares  as  nodes,  and  the  connections  between  squares  as  the 
edges  of  the  graph.  Most  of  these  methods  can  be  adapted  to 
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the  three-dimensional  path-planning  problem  in  one  form  or 
another. 

3.  Path  Planning  in  Three-Dimensions 

The  current  approaches  to  the  three-dimensional  path¬ 
planning  problem  center  on  a  three-dimensional  version  of  the 
two-dimensional  visibility  graphs  or  involve  attempts  to 
decompose  the  three-dimensional  problem  to  a  two-dimensional 
problem  [Ref.  6].  This  thesis  is  unique  in  approaching  the 
problem  by  reducing  the  size  of  the  search  space  by  grouping 
possible  graph  nodes,  and  then  applying  a  simple  search  to 
find  an  optimal  path. 

4.  Fermat's  Principle 

Fermat's  Principle,  first  proposed  by  Pierre  de 
Fermat  in  1657,  states  that  light  rays  travelling  through  an 
optical  medium  will  always  take  the  path  which  can  be 
traveled  in  the  least  amount  of  time  [Ref.  8].  As  light 
passes  through  several  layers  of  different  optical  mediums, 
then,  it  will  travel  along  a  path  with  the  shortest  optical 
path  length.  Fermat's  Principle  leads  to  Snell's  Law,  which 
states  that 

Nl*sinfil=N2*sinB2  (2-1) 


where 

N1  is  the  index  of  refraction  of  material  #1, 

81  is  the  angle  of  incidence,  measured  from  the 
normal , 
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N2  is  the  index  of  refraction  of  material  #2,  and 

&2  is  the  angle  of  refraction,  measured  in  a  counter¬ 
clockwise  sense  from  the  normal. 

Figure  6  illustrates  this  effect.  Fermat's  Principle 
provides  a  useful  method  of  minimizing  the  total  transit  time 
of  a  path  between  start  and  goal  points.  Assuming  that  the 
flight  path  is  flown  at  a  constant  speed,  this  minimized 
transit  time  can  be  multiplied  by  the  speed  (a  constant)  to 
produce  a  minimized  distance: 

Distance=Time*Speed  (2-2) 

As  a  result,  all  Snell’s  Law  optimizations  will  be 
discussed  as  optimization  of  distance,  even  though  time  is 
actually  the  variable  optimized.  Note  that  this  is  only  true 
if  the  speed  is  a  constant,  and  if  no  other  costs  are 
accounted  for  at  the  turn-point.  If  turn  costs  were  to  be 
included  in  the  Snell's  Law  optimization,  then  Equation  2-2 
will  not  hold. 

The  index  of  refraction  used  in  Snell's  Law  can  be 
constrained  to  be  the  probability-of-detection  of  a 
particular  visibility  volume,  so  that  a  path  through  a  series 
of  visibility  volumes  can  be  considered  analogous  to  a  light 
ray  passing  through  a  series  of  optical  mediums.  Minimizing 
the  deviation  from  Snell's  Law  at  each  transition  layer 
between  volumes  will  guarantee  minimization  of  the  total  path 
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Material  #1 


Material  #2 


Figure  6.  Snell's  Law 


length  because  minimization  of  Equation  2-1  is  convex  [Ref. 
4]. 

5.  Visibility  Models 

A  significant  amount  of  prior  research  has  been  done 
in  the  area  of  visibility  and  visibility  algorithms.  The 
three-dimensional  path-planning  program  requires  an  all- 
around  visibility  model.  Fortunately,  some  aspects  of 
standard  graphics  approaches  to  visibility  can  be  modified 
for  use  for  this  model.  [Ref.  10]  refers  to  finding  shadow 
regions  of  polyhedral  solids  by  projecting  planes  from  the 
observer  to  the  edges  of  a  shadowed  object,  creating  a  shadow 
polygon  behind  the  shadowed  object. 

B .  MATHEMATICS  BACKGROUND 
1.  Planes 

A  quick  overview  of  some  basic  three-dimensional 
mathematics  is  in  order  at  this  point.  A  plane  in  space  is 
represented  by  an  equation  of  the  form 

Ax+By+Cx=Ao  ( 2 - 3 ) 

where  A,  B  and  C  are  the  coefficients  of  the  vector  N  normal 
to  the  surface  of  the  plane. 

N=Ai+Bj+Ck  (2-4) 

and  AO  is  found  by  substituting  the  coordinates  of  any  point 
in  the  plane  into  the  left  hand  side  of  Equation  2-3  [Ref. 
11].  Figure  7a  illustrates  this  arrangement. 
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Plane 


Plane  Representation 

(a) 


Line  Segment  Representation 
(b) 


Figure  7.  Lines  and  Planes 

0 


2 


2 .  Lines 


A  line  in  space  has  a  more  complicated 
representation.  Any  line  in  space  is  completely  defined  by 
two  vectors:  a  position  vector  leading  from  the  origin  to 

any  point  on  the  line,  and  a  direction  vector  pointing  along 
the  direction  of  the  line.  The  coordinates  of  any  point 
along  the  line  can  be  determined  by  changing  the  coefficient 
{or  parameter)  t  in  the  line  equation 

P(x,y,z)=A+B+C+t*(D+E+F)  (2-5) 

where  A,  B  and  C  are  the  coefficients  of  the  position  vector, 
and  D,  E  and  F  are  the  coefficients  of  the  direction  vector 

Position  vector=Ai+Bj+Ck  (2-6) 

Direction  Vector=Di+Ej+Fk.  (2-7) 

This  is  called  the  parametric  form  of  a  line  in  space  [Ref. 
11],  and  is  illustrated  in  Figure  7b. 

The  parametric  form  of  a  line  is  very  useful  since 
any  single  point  along  the  line  may  be  specified  by  a  single 
parameter,  rather  than  two  or  three  different  variables.  A 
line  segment  can  thus  be  easily  defined  by  means  of  two 
parameters,  one  each  for  the  endpoint  of  the  line  segment. 
If  the  parameter  at  one  endpoint  is  arbitrarily  set  to  zero, 
then  the  entire  line  segment  can  be  represented  by  a  single 
parameter  and  some  constants. 
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3 .  Volumes 


Figure  8  illustrates  the  structure  of  solid 
polyhedra,  or  volumes,  used  in  this  thesis.  The 
relationships  between  volumes,  points,  edges,  facets  and 
planes  can  be  best  Illustrated  by  the  hierarchical  graph  of 
Figure  9.  These  relationships  form  the  foundation  for  the 
subsequent  development  of  the  data  types  to  be  used  to 
represent  these  objects  during  the  implementation  of  the  path 
planning  program.  These  data  structures  will  be  presented  in 
detail  in  Chapter  IV. 


Edge  (Line  segment) 


Figure  8.  Volume  Structure 
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Figure  9.  Relationships  Among  Solid-Geometry  Data 
Structures 
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III.  VISIBILITY  AND  PATH-PLANNING  ALGORITHMS 


This  chapter  will  describe  the  algorithms  used  for  three- 
dimensional  path  planning.  Most  of  this  chapter  will  discuss 
visibility  determination  and  path  planning.  Later  sections 
will  present  algorithms  in  solid  geometry. 

As  discussed  in  Chapter  l,  a  fundamental  concept  in  this 
work  is  the  application  of  a  grouping,  or  coalescing  criteria 
to  three-dimensional  volumes  in  the  three-dimensional  path 
planning  problem,  using  one  or  more  common  characteristics 
within  large  regions  of  airspace  to  simplify  the  search 
problem.  The  common  characteristic  used  here  will  be 
visibility. 

The  three-dimensional  path-planning  algorithm  breaks 
naturally  into  two  halves:  static-visibility  determination 
and  path  planning.  The  static-visibility  determination  phase 
uses  the  grouping  characteristic  (visibility)  to  find 
visibility  volumes,  and  then  builds  a  search  graph.  Path 
planning  performs  the  search  through  the  search  graph,  which 
generates  the  volume  path  defined  in  Chapter  I.  The  path¬ 
planning  algorithm  must  then  find  the  final  optimal 
piecewise-linear  path  through  the  volume  path. 
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A.  VISIBILITY-REGION  CONSTRUCTION 

1 .  Terrain  Model 

A  polyhedral  model  of  terrain  will  be  used  for  path 
planning  to  simplify  the  mathematics  of  the  visibility 
algorithms.  The  terrain  model  used  is  described  in  [Ref.  9]. 
This  type  of  terrain  model  produces  large  planar  patches 
which  are  as  large  as  possible  given  the  terrain,  not  the 
simple  triangular  terrain  patches  found  in  more  common 
polyhedral  models.  Figure  10a  shows  some  sample  redl 
terrain,  and  Figure  10b  shows  the  more  general  polyhedral 
model  used.  Figure  10b  also  shows  the  structural  lines  used 
to  create  a  base  for  the  terrain  model.  These  vertical  and 
horizontal  edges  simply  outline  the  sides  and  bottom  of  the 
terrain  to  create  a  ground  volume,  which  can  be  manipulated 
in  the  same  way  that  any  air  volume  can  be  manipulated, 
creation  of  these  lines  is  discussed  in  the  following 
chapter . 

2.  Static  Visibility-Deteraination  Algorithm 

The  location  and  extent  of  the  visibility  regions  is 
completely  determined  by  the  geometry  of  the  terrain  and  the 
location  of  observers  relative  to  that  terrain.  Regions  of 
visibility  (regions  of  non-zero  probability-of-detection)  are 
regions  of  points  which  have  an  unobstructed  line-of-sight 
(LOS)  from  an  observer.  Figure  11  shows  cross-sections  of 
two  different  examples  of  such  visibility  regions. 
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(a) 


Structuxttl  Lines 


Observers 


(b) 


Figure  10. 


Terrain  Modeling 
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a.  Deternination  of  Visibility  Regions 

Non-visible  regions  (with  respect  to  a  single 
observer)  are  simply  regions  shadowed  from  the  observer  by 
some  terrain  feature.  Many  shadowing  algorithms  are 
available,  as  discussed  in  Chapter  Two,  but  most  are  oriented 
towards  views  of  terrain  from  particular  vantage  points,  and 
generally  do  not  involve  construction  of  shadow  volumes, 
some  shadowing  algorithms  [Ref.  10]  do  provide  useful  methods 
which  can  be  extended  to  the  full  shadowing  problem  with 
great  success.  These  methods  involve  constructions  between 
an  observer  and  significant  features  of  the  surrounding 
terrain.  Note  in  Figure  11  that  the  limits  of  the  visibility 
regions  are  determined  by  a  linear  construction  between  the 
observer  and  the  points  of  peaks.  In  three  dimensions  (using 
polyhedral  approximations  to  real  terrain),  these  peaks  would 
be  ridge  lines,  and  the  linear  construction  would  be  a  plane 
(see  Figure  12)  called  the  limiting  plane  of  visibility). 

This  limiting  plane  of  visibility  is  easily 
found,  since  a  plane  can  be  defined  by  a  point  (the  observer) 
and  a  line  (the  ridge  line).  As  a  result,  the  visibility 
regions  may  be  built  using  the  following  algorithm: 
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Ridge  Line 


\ 


Observer 


Initial  Condition:  A  large  volume  of  air  above  some  terrain 
LOOP  for  Observer  IN  (all  observers) 

LOOP  for  Ridge-line  IN  (all  ridge-lines) 

CONSTRUCT  Limiting  Plane  of  Visibility  using 
Observer  and  Ridge-line 

DIVIDE  all  volumes  at  the  Limiting  Plane  of 
visibility 

END 

END 

Algorithm  3-1.  Construction  of  Visibility  Volumes 

Results  of  this  algorithm  are  illustrated  in  a 
cross-sectional  illustration  in  Figure  13a.  Note  that  this 
algorithm  will  construct  limiting  planes  of  visibility 
regardless  of  whether  the  ridge  line  is  visible  from  the 
given  observer,  which  creates  extra  work,  A  better  algorithm 
would  consider  this  before  building  the  limiting  planes  of 
visibility: 


Initial  Condition:  A  large  volume  of  air  above  some  terrain 

LOOP  for  Observer  IN  (all  observers) 

LOOP  for  Ridge-line  IN  (all  ridge-lines) 

IF  ridge  line  visible  from  Observer  THEN 

CONSTRUCT  Limiting  Plane  of  Visibility  using 
Observer  and  Ridge-line 

DIVIDE  all  volumes  at  the  Limiting  Plane  of 
visibility 


END 


END 


END 


Algorithm  3-2.  Better  Construction  of  Visibility  Volumes 
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Illustrative  results  for  this  algorithm  are  shown 
in  Figure  13b.  This  algorithm  was  not  implemented  in  this 
thesis . 

b.  Visibility  Criteria 

After  the  visibility  volumes  are  built,  their 
visibility  relative  to  all  observers  must  be  determined. 
This  is  best  done  using  a  simple  line-of-sight  (LOS)  model. 
Since  the  visibility  volumes  define  regions  of  uniform 
visibility,  no  point  in  the  volume  is  more  or  less  visible 
than  any  other  point.  For  this  reason,  visibility  can  be 
determined  from  the  center  of  the  volume,  which  is  easy  to 
find,  to  an  observer.  If  that  LOS  is  blocked  by  terrain 
anywhere  along  its  length,  then  the  entire  visibility  volume 
is  not  visible  with  respect  to  that  particular  observer,  see 
Figure  14  for  an  example. 

Once  the  visibility  with  respect  to  all  observers 
has  been  calculated,  the  probability-of-detection  for  all 
volumes  may  be  calculated  in  some  appropriate  manner.  This 
is  a  function  of  the  individual  probabilities-of-detection 
for  all  of  the  observers  which  "car,  see"  a  given  visibility 
volume,  and  is  discussed  in  detail  in  the  following  chapter 
and  in  [Ref.  2].  Volumes  which  are  visible  to  no  observer 
can  be  assigned  a  probability-of-detection  of  zero,  or  some 
small,  non-zero  baseline  value. 


32 


•  >  Cenler  of  a  Volume  - >  Line  of  Sight 


•  Volume  #1  is  visible  from  Observer  #1  and 

not  visible  from  Observer  #2 

•  Volume  #2*  is  not  visible  from  Observer  #1  and 

not  visible  from  Observer  #2 

•  Volume  #3  is  visible  to  both  observers 

•  Volume  #4  is  visible  to  Observer  #2  and 

not  visible  to  Observer  #1 


Figure  14.  Visibility  Criteria 
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c.  Additional  Sub-algorithms 

Several  other  sub-algorithms  are  required  by  the 
static-visibility  determination  algorithm;  input,  ridge¬ 
finding  and  graph-building.  The  input  algorithm  is  highly 
dependent  upon  the  method  of  representing  the  polyhedral 
terrain  model,  and  its  discussion  will  be  delayed  until 
Chapter  IV. 

(1)  Ridge  Finding.  Implicit  in  the  visibility 
volume  algorithm  is  the  ability  to  identify  ridge  lines, 
information  not  (generally)  available  with  the  input  data.  A 
ridge  line  is  a  line  segment  at  the  intersection  of  two 
facets  of  the  terrain  model  such  that  the  two  facets  are 
concave  down  (see  Figure  15a).  A  complex  formula  is 
available  to  do  this  in  three  dimensions  [Ref.  12]  which  is 
completely  general.  However,  it  is  possible  to  take 
advantage  of  the  polyhedral  representation  of  the  terrain  to 
make  a  much  easier  algorithm. 

First  project  the  center  point  of  one  of  the 
facets  (it  does  not  matter  which)  onto  the  plane  of  the  other 
facet.  If  that  projection  is  above  the  center  point  (e.g., 
has  a  higher  altitude,  or  z  value),  the  line  is  a  ridge  line. 
If  any  other  case  arises,  then  the  line  is  not  a  ridge  line. 
Figure  15b  illustrates  this  essentially  graphical  approach. 
Exactly  vertical  cliffs  are  not  considered  here,  since  they 
are  assumed  not  to  occur  in  nature.  This  also  serves  to 
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avoid  identifying  structural  edges  used  to  bound  the  terrain 
model  (the  top,  sides  and  bottom)  as  ridge  lines. 

(2)  Graph  Building.  The  earlier  steps  in  the 
static-visibility  algorithm  produce  a  network  of  adjacent 
visibility  volumes.  These  volumes  will  be  connected  to  their 
immediate  neighbors  by  either  a  common  facet  (the  usual 
case),  or  adjacent,  coplanar  facets  (an  unusual, 
implementation-dependent  case).  In  either  event,  the 
connecting  facet  represents  an  edge  in  the  search  graph,  and 
the  centers  of  the  visibility  volumes  represent  nodes  in  the 
graph.  Visibility  volumes  are  not  considered  to  connect  at 
common  volume  vertices  or  volume  edges.  Building  the  search 
graph  involves  a  very  straight  forward  algorithm  to  find  all 
connections  between  all  visibility  volumes: 


LOOP  for  volume  IN  (all  visibility  volumes) 

LOOP  for  facet  IN  (all  facets  of  a  volume) 

FIND  other  volumes  CONTAINING  facet 

CONCLUDE  THAT  volume  CONNECTS  to 
Other  volumes 

END 

END 


Algorithm  3-3.  Algorithm  for  Finding  Search-Graph 

Edges 


B.  PATH  PLANNING  ALGORITHMS 

Our  path-planning  is  a  two-step  process.  First  an  A* 
search  algorithm  is  used  to  find  the  first  optimal  volume 
path  from  the  start  volume  (the  volume  containing  the  start 
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point)  to  the  goal  volume  through  the  search  graph. 
Quantitative  factors  are  included  in  this  first  step,  such  as 
probability-of-detection  and  turn  cost.  This  step  finds  an 
optimal  set  of  contiguous  volumes  through  which  the  final 
path  must  move.  This  is  the  only  step  in  the  path  planning 
process  in  which  aerodynamic  processes,  such  as  climbs  and 
turns,  are  explicitly  manipulated. 

The  second  step  finds  an  optimal  piecewise-linear  path 
within  the  confines  of  the  volume  path  (see  Figure  5,  in 
Chapter  I).  In  order  to  reduce  the  complexity  of  the  path 
planning  process,  only  distance  flown  and  probability-of- 
detection  are  optimized,  using  Snell's  Law  as  an  optimizing 
tool.  For  reasons  to  be  described  later  in  this  section, 
maneuvers  (changes  in  direction  or  in  angle  of  climb/dive) 
are  restricted  to  the  boundaries  between  volumes,  so  the  path 
produced  by  this  step  is  a  series  of  linear  path  segments, 
joined  at  the  boundaries  between  volumes. 

1 .  Volume-oriented  A*  Search 

The  classic  A*  method  has  been  used  in  this  thesis  to 
find  a  "reasonable"  flight  path  with  a  trade-off  between 
minimum  cost  and  minimum  probability-of-detection.  A 
reasonable  flight  path  will  be  defined  as  a  path  in  which  an 
appropriate  amount  of  effort  has  been  made  to  avoid  regions 
with  a  high  probability-of-detection,  while  not  going  too  far 
off  of  a  minimum  cost  path.  As  an  example,  when  driving  from 
Los  Angeles,  California  to  Phoenix,  Arizona  while  minimizing 


cost  and  trying  to  avoid  desert  terrain,  it  can  be  considered 
reasonable  to  go  via  San  Diego  and  Yuma  (see  Figure  16).  It 
would  not  be  reasonable  to  go  via  Barstow  and  Flagstaff,  or 
via  Sacramento  and  Denver. 

As  mentioned  earlier,  heuristics  are  a  key  tool  in 
deciding  the  type  of  path  to  be  found.  [Ref.  2]  provides  an 
excellent  discussion  of  the  role  of  heuristics  in  path 
planning.  Heuristics  used  here  are: 

•  Remain  in  volumes  with  low  probabilities-of-detection  as 
long  as  possible. 

•  Fly  low  whenever  possible. 

•  Minimize  distance  flown. 

•  Small  turns  are  better  than  large  turns. 

These  will  be  implemented  as  adjustments  to  the  cost 
and  evaluation  functions  of  the  A*  search.  Note  that  many  of 
these  heuristics  can  conflict  with  one  another,  such  as  the 
first  and  third  ones.  It  is  up  to  A*  search  to  resolve  these 
conflicts  in  a  "correct"  way. 

A*  search  requires  both  a  cost  function  and 
evaluation  functions,  as  described  in  Chapter  II,  Section  A1 . 
since  the  A*  search  is  finding  a  volume  path  through  the 
search  graph,  estimators  of  energy  use  rather  than  exact 
formulas  are  used.  These  estimators  account  for  the  extra 
costs  incurred  when  turning  or  climbing  an  aircraft.  Energy 
"savings"  (e.g.,  reduced  costs  over  equivalent  level  flight) 
can  be  expected  when  diving. 
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Figure  16.  Optimum  Routing  Example 


In  order  to  include  non-energy  factors  in  the  A* 
search  (such  as  probability-of-detection) ,  they  are  used  to 
arithmetically  modify  the  results  of  the  cost  and  evaluation 
functions  to  reflect  the  "desirability”  of  being  in  a  region 
with  a  given  probability-of-detection.  Thus  regions  with  a 
high  probability-of-detection  will  incur  a  proportionally 
higher  energy  use  for  a  given  flight  path  than  a  similar 
region  with  a  lower  probability-of-detection.  This  means 
that  no  separate  decision  must  be  made  to  select  path  volumes 
based  solely  on  probability-of-detection  information, 
a.  Compensated  Cost 

One  approach  to  figuring  costs  when  using  the  A* 
search  is  to  calculate  path-segment  costs  based  on  volume- 
center  to  volume-center  distances,  as  shown  in  Figure  17. 
Turn  and  altitude  energy  estimators  can  be  calculated  from 
the  horizontal  and  vertical  deviation  of  the  current  center- 
to-center  path-segment  direction  from  the  previous  center-to- 
center  path-segment  direction. 

The  modification  to  the  actual  cost  of  a  path 
segment  (volume-center  to  volume-center  distance)  for 
probability-of-detection  can  be  expressed  as: 

Net  Cost= (Actual  Cost)  *  PD-Modifier  (3-1) 

where  "PD-Modifier"  is  some  function  of  the  probability-of- 
detection. 
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•  start  Point  at  "S" 

•  Goal  Point  at  "G" 

•  Visibility  Volumes  named  "VI"  through  "V9" 

•  Volume  center-to-center  paths  marked  by 


Figure  17.  Volume  Center  Paths 
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For  a  flying  object,  certain  maneuvers  represent 
a  certain  fixed  cost.  A  turn  will  incur  such  a  fixed  cost, 
which  is  a  function  of  the  speed  of  the  flying  object  and  the 
degree  of  turn.  Increased  turn  angles  will  incur  increased 
fixed  turn  costs,  up  to  some  maximum  allowed  turn  angle. 
This  can  be  repeated  in  simplified  form  by  modifying  Equation 
3-1  as  follows: 

Net  Cost= (Actual  Cost  +  Turn  Cost)  *  PD-Modifier  (3-2) 

Finally,  flying  objects  use  extra  energy  when 
climbing,  at  a  rate  which  is  a  function  of  the  length  of  the 
climb  (the  Actual  Cost)  and  the  degree  of  the  climb.  They 
also  use  less  energy  when  diving,  again  as  a  function  of  the 
length  and  steepness  of  the  dive.  A  final  change  must  then 
be  made  to  Equation  3-1  which  represents  a  modification  for 
altitude  changes.  Note  that  this  modifier  must  be  a 
multiplicative  modifier,  since  the  energy  gain/loss  is  a 
function  of  the  length  of  the  climb  or  dive. 

Net  Cost= (Actual  Cost  +  Turn)  * 

^  Altitude-Modifier  *  PD-Modifier  (3-3) 

Equation  3-3  represents  the  total  compensated 
cost  for  a  path  segment.  It  can  be  used  when  calculating 

both  the  cost  and  evaluation  functions  during  the  A*  search, 
b.  Search  Algorithm 

The  classic  A*  search  algorithm,  as  modified  for 
this  thesis,  begins  with  an  agenda  initialized  to  all 
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successors  (adjacent  volumes)  of  the  volume  containing  the 
start  point,  and  all  cost  and  evaluation  functions 
precalculated  for  those  successor  volumes.  Then: 

LOOP  UNTIL  goal-volume  is  on  the  agenda 

REMOVE  the  best  successor  from  the  agenda 

FIND  all  successor  volumes  to  the  best  successor 

UPDATE  path  information  for  the  successor  volumes 

CALCULATE  cost  and  evaluation  functions  for  the 
successor  volumes 

INSERT  all  successor  volumes  onto  the  agenda 

END 

Algorithm  3-4.  Search  Algorithm  for  Finding  Volume  Path 

The  A*  evaluation  function  calculates  the 
compensated  cost  for  the  distance  from  the  center  of  the 
successor  volume  to  the  goal,  ignoring  obstructions,  and 
assuming  that  the  probability-of-detection  of  the  successor 
volume  applies  to  the  entire  distance  to  the  goal.  Note  that 
this  evaluation  function  is  not  the  lower-bound  evaluation 
function.  The  cost  function  is  a  simple  summation  of  all 
compensated  costs  incurred  up  to  the  current  volume  along  the 
current  path.  Adding  these  cost  and  evaluation  functions 
gives  the  total  cost  for  that  successor  volume 
c.  Optimal  Plecewise-Linear  Path 

The  optimal  piecewise-linear  path  is  the  minimum- 
cost  path,  consistent  with  the  objective  of  maintaining  the 
lowest  possible  probability-of-detection  along  the  path, 
which  lies  entirely  within  the  volume  path  (see  Figure  18). 
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In  order  to  reduce  the  complexity  of  the  path  planning 
process,  this  minimum  cost  is  not  modified  in  any  way  by  the 
maneuvers  along  the  flight  path,  but  is  simply  the  sum  of  the 
point-to-point  distances  along  each  path-segment  of  the  total 
path.  The  only  constraints  placed  on  the  linear  path  are 
that  it  must  be  contained  entirely  within  the  volumes 
comprising  the  volume  path  (since  this  was  determined  to  be 
the  optimal  way  to  go) ,  that  maneuvers  occur  only  at  the 
edges  of  volumes,  and  that  the  distances  traveled  in  volumes 
with  high  probabilities-of-detection  be  minimized. 

The  first  two  constraints  can  be  met  if  all 
visibility  volumes  are  convex,  and  all  linear  path  line 
segments  are  "built"  between  facets.  The  convex  structure  of 
visibility  volumes  will  ensure  that  any  line  segments 
connecting  facets  of  the  volume  will  always  lie  completely 
inside  the  volume  [Ref.  13].  Ensuring  that  maneuvers  occur 
only  at  volume  facets  is  an  implementation  issue. 

At  this  point,  it  may  be  useful  for  the  reader  to 
discard  the  concept  of  visibility  volumes,  and  consider  the 
construction  of  an  optimal  linear  flight  path  as  a  situation 
in  which  a  flight  path  must  be  found  from  a  start  point 
through  a  series  of  restrictive  windows  (see  Figure  19)  to 
the  goal  [Ref.  14].  The  windows,  in  this  case,  are  the 
facets  which  connect  the  volumes  of  the  volume  path. 
Probability-of-detection  information  must  still  reside  in  the 
spaces  between  the  windows.  These  windows,  or  facets,  will 
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be  called  maneuver  facets,  reinforcing  the  requirement  that 
all  maneuvers  must  be  conducted  within  the  plane  of  these 
facets . 

Before  proceeding,  an  initial  guess  at  the 
piecewise-linear  optimal  path  is  needed.  Any  suitable  choice 
will  do,  so  a  very  general  maneuver-facet-center  to  maneuver- 
facet-center  path  can  be  selected  as  the  initial  guess. 

The  third  constraint,  the  need  to  minimize  the 
distance  traveled  through  high  probability-of-detection 
volumes,  can  be  handled  by  applying  an  adaption  of  Snell's 
Law.  Using  the  probability-of-detection  of  a  volume  as  the 
index  of  refraction  for  Snell's  Law,  the  angles  of  incidence, 
and  refraction  as  shown  in  Figure  20,  Snell's  Law  will  be: 

(Pdl  +  l)*sin  Bl=(Pd2  +  l)*sin  B2  (3-4) 


or 


(Pdl  +  l)*sin  Bl  -  (Pd2  +  l}*s±n  B2  =  0  (3-5) 

or,  if  Snell's  law  is  not  exactly  met, 

(Pdl  +  l)*sin  B1  -  (Pd2  +  l)*sin  B2  =  e  (3-6) 

where 


Pdl  is  the  probability-of-detection  on  the  incident 
side , 

Pd2  is  the  probability-of-detection  on  the  refracted 
side , 

fil  is  the  incident  angle. 
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(a) 


(b) 


Figure  20.  Snell's  Law  Optimization  Situations 
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B2  is  the  refracted  angle,  and 
e  is  an  error  item. 


The  index-of-ref raction  of  Equation  2-1  is 
represented  by  one  plus  the  probability-of-detection.  This 
is  simply  to  avoid  having  an  index-of-refraction  of  zero  when 
Pdl  or  Pd2  is  zero.  Note  that  the  two  coefficients  (Pdl+1) 
and  (Pd2+1)  are  constant,  and  that  fil  and  fi2  can  be  varied  by 
moving  the  path  point  on  the  maneuver  facet  (see  Figure  20), 
assuming  that  the  other  ends  of  the  path  lines  remain  fixed. 
Note  also  that  £1  and  62  are  actually  functions  of  the 
position  that  the  path  intersects  the  maneuver  facet,  called 
the  path  point  or  maneuver  point.  As  the  maneuver  point  is 
moved,  the  values  of  61  and  62  will  change,  and  the  value  of 
the  error  in  Equation  3-6  will  also  change.  Only  when 
Snell's  Law  is  exactly  satisfied  will  Equations  3-5  and  3-6 
be  equal.  Figure  20b  does  not  correspond  to  a  situation 
found  in  optics,  but  such  an  arrangement  does  occur  in  path 
planning . 

If  the  maneuver  point  is  constrained  to  move 
along  a  line  within  the  maneuver  facet  (only),  then  Snell's 
Law  will  guarantee  that  a  minimum  value  for  the  error  in 
Equation  3-6  can  be  found  ([Ref.  3]  and  [Ref.  4]).  If  one 
notes  that  the  path  segments  LI  and  L2  of  Figure  20  must  lie 
in  a  plane,  then  the  movement  of  the  maneuver  point  can  be 
constrained  to  lie  in  the  line  defined  by  the  intersection  of 
the  plane  of  the  maneuver  facet  and  the  plane  formed  by  LI 
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and  L2 .  Further  restricting  this  line  to  be  a  line  segment 
contained  within  the  maneuver  facet  will  ensure  that  a 
minimum  value  for  the  error  term  in  Equation  3-6  can  be  found 
within  the  maneuver  facet. 

Finding  the  optimal  linear  path,  then,  involves 
applying  the  above  Snell 's-Law  optimization  to  each  maneuver 
facet  along  the  volume  path  in  turn,  keeping  all  other 
maneuver  points  fixed.  A  single  optimization  pass  occurs 
when  all  maneuver  points  have  been  adjusted  one.  The 
optimization  passes  must  then  be  repeated  until  the  length  of 
the  entire  linear  path  converges  to  some  optimal  value. 

C.  SUPPORT  ALGORITHMS 

1 .  Intersection  of  a  Plane  and  a  Volume  in  Space 

The  intersection  of  a  plane  with  an  arbitrary 
polyhedral  (planar)  volume  in  three-dimensional  space  depends 
on  the  shape  of  the  volume.  For  the  purposes  of  this 
discussion,  volumes  may  be  classified  into  two  general  types: 
convex  and  non-convex  planar  volumes. 

a.  Interception  with  Convex  Volumes 

Figure  21a  shows  that  the  interception  of  a 
convex  volume  will  create  two  smaller  resultant  volumes. 
These  resultant  volumes  are  composed  of  whole  and  partial 
edges  from  the  original  volume  plus  new  edges  created  by  the 
intersection  process  itself.  The  newly  created  edges  always 
lie  in  the  intercepting  plane,  and  in  a  plane  of  one  of  the 
other  facets  of  the  original  volume.  In  general,  the  facets 
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Lower  Resultant  Volume 


Simple  Convex  Volume  Intersection 

(a) 


{b) 


Figure  21.  Volume  Intersections 


of  the  original  volume  will  not  be  unaffected  by  the 
interception;  several  will  be  subdivided  by  the  process. 


There  are  several  "degenerate"  cases  of 
interception  shown  in  Figure  22.  Note  that  the  original 
volume  lies  entirely  on  one  side  of  the  intersecting  plane. 
This  observation  will  be  useful  in  detecting  these  cases, 
b.  Alternative  Algorithms 

While  several  methods  have  been  presented  to 
intersect  a  plane  with  a  volume  [Ref.  10],  two  approaches 
immediately  suggest  themselves.  The  first  finds  the  lines 
defined  by  the  intersection  of  the  plane  and  the  planes  of 
the  facets  of  the  volume,  and  then  fits  these  "lines  of 
intersection"  onto  the  facets.  The  second  approach  finds  the 
points  defined  by  the  intersection  of  each  edge  of  the  volume 
with  the  intersecting  plane,  and  connects  these  points  along 
the  facets  of  the  volume. 

The  first  approach  was  not  used  due  to  the 
complexity  of  the  fitting  procedure.  The  second  approach, 
the  "plane-edge  method,"  involves  a  simpler  intersection  of 
edges  and  a  plane.  Each  edge  of  the  original  volume  is 
intercepted  with  the  intersecting  plane  in  turn,  possibly 
subdivided  according  to  the  results  of  the  interception,  and 
then  placed  in  the  appropriate  resultant  volume.  Edges  that 
lie  in  the  plane  of  the  interception  are  found  or  built  by 
connecting  the  intersection  points  of  the  edges  and  the  plane 
in  an  appropriate  manner,  and  then  placed  in  both  resultant 
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Vertex  Intecept 
(c) 


Figure  22.  Degenerate  Intercept  Cases 
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volumes.  Finally  the  resultant  volumes  are  checked  for  the 
degenerate  conditions  that  indicate  that  a  partial  intercept 
has  occurred. 

(1)  Intersection  of  a  Line  and  a  Plane.  The 
intersection  point  of  a  generalized  line  and  a  plane  in  three 
space  is  derived  from  the  equation  of  a  plane  [Ref.  11] 

ax  +  by  +  cz  =  Ao  (3-7) 

and  the  vector-parametric  representation  for  a  edge  described 
in  Chapter  II  [Ref.  11] 

r(t)  =  Ai  +  Bj  +  Ck  +  t(Di  +  Ej  +  Fk)  (3-8) 

where 

Ai  +  Bj  +  Ck  is  the  equation  of  the  position  vector. 

Di  +  Ej  +  Fk  is  the  equation  of  the  direction  vector. 

t  =  0  at  one  end-point  of  the  edge  and 

t  =  a  maximum  value  at  the  other  end-point  of  the 
edge  (called  "t-max"). 

As  a  result,  all  points  on  the  line  are  given  by  the 
parametric  equations 

X  =  A  +  tD  (3-9) 

y  =  B  +  tE  (3-10) 

z  =  C  +  tF  (3-11) 
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Solving  these  equations  simultaneously  yields 


t  =  [Ao  -  (aA  +  bB  +  cC)]/[aD  +  bE  +  cF]  (3-15) 


at  the  intercept  point,  which  is  on  the  line  defined  in 
Equation  3-8. 

(2)  Intersection  of  a  Edge  and  a  Plane. 
Intercepts  between  a  edge  and  a  plane  may  result  in  one  of 
three  situations: 

•  0  <  t  <  t-max:  This  indicates  that  the  intercept 

occurred  within  the  edge.  If  t 
equals  either  0  or  t-max,  then  the 
intercept  occurs  at  one  of  the 
endpoints,  otherwise  it  occurs  at 
some  middle  point  along  the  edge. 

•  t  <  0  or  t  >  t-max:  This  indicates  that  the  intercept 

has  occurred  beyond  the  endpoints  of 
the  edge.  No  intercept  occurs, 

•  t  is  infinite:  This  occurs  when  the  denominator  of 

Equation  3-15  equal  zero.  In  this 
case,  the  edge  is  parallel  to  the 

intersecting  plane.  Again,  no 

intercept  occurs. 

(3)  Building  the  Resultant  Volumes.  Using  the 

assumption  that  two  new  volumes  will  be  created  upon 
intersection  of  a  convex  volume  by  a  plane,  the  results  of 
the  line-plane  intersection  described  above  can  be  used  to 
create  the  resultant  volumes  as  the  process  occurs  for  each 
edge  of  the  volume.  In  the  case  that  no  intercept  occurs  for 

a  particular  edge  (the  second  and  third  cases  above)  one 

point  of  the  edge  must  be  checked  for  its  location  relative 
to  the  intersecting  plane,  then  added  to  the  appropriate 
resultant  volume.  For  an  intercept  at  an  endpoint  of  an 
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edge,  the  other  endpoint  must  be  checked  for  its  location 
relative  to  the  intersecting  plane,  and  then  the  unchanged 
edge  may  be  placed  in  the  appropriate  resultant  volume. 

When  the  plane  intersects  the  edge  anywhere 
in  its  middle,  the  edge  must  be  divided  into  two  smaller 
edges.  These  may  be  placed  into  the  resultant  volumes  using 
the  procedure  used  for  an  intersection  at  an  end  point. 

New  edges  must  be  built  in  the  plane  of  the 
intercepting  plane.  Note  that  any  lines  or  points  in  this 
plane  belong  in  both  resultant  volumes.  The  algorithm  must 
find  the  edges  which  connect  the  intercept  points  in  this 
region,  within  the  constraints  of  the  particular 
construction  of  the  original  volume.  These  new  edges  demark 
the  effect  of  the  intersecting  plane  passing  through  the 
original  volume,  and  construction  of  these  edges  involves 
connection  of  interception  points  in  the  planes  of  the  facets 
of  the  original  volume. 

Now  it  is  possible  to  check  for  the 
degenerate  intercept  conditions  discussed  earlier.  If  an 
intercept  occurred  then  both  of  the  resultant  volumes  must 
not  be  degenerate  volumes.  To  be  a  valid  volume  (non- 
degenerate)  several  Independent  minimum  conditions  must  be 
met.  The  resultant  volumes  must 

•  have  at  least  four  points 

•  have  at  least  six  edges 

•  have  at  least  four  facets 
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meet  Euler's  Relation  [Ref.  15], 


V  +  R  -  E  =  2 


(3-15) 


where 


E  is  the  number  of  edges  in  the  volume, 

V  is  the  number  of  vertices  in  the  volume,  and 
R  is  the  number  of  regions  (facets)  in  the  volume 

If  either  of  the  resultant  volumes  does  not 
meet  any  of  these  degeneracy  checks,  then  the  original  volume 
is  unpartioned  by  the  intersection  process. 

c.  Intersection  with  Non-Convex  Volumes 

A  non-convex  planar  volume  is  one  with  at  least 
one  non-convex  surface  feature.  Intersection  of  such  a 
volume  with  a  plane  will  produce  at  least  two  resultant 
volumes,  but  often  more  (see  Figure  21b).  It  is  this 
inability  to  determine  exactly  how  many  resultant  volumes 
which  will  be  produced,  and  the  inability  to  predict  their 
location  relative  to  the  intersecting  plane,  that  makes 
interception  of  this  type  of  volume  difficult. 

Fortunately,  only  one  subcase  of  this  problem 
arises  when  dealing  with  polyhedral  terrain  models.  This  is 
illustrated  in  Figure  23.  This  special  situation  is  easily 
identified  by  adding  a  step  to  the  intersection  algorithm  for 
all  new  edges  created  in  the  plane  of  the  intercepting  plane 
that  tests  that  some  non-endpoint  point  on  the  new  edge  (such 
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Unwanted  Line  Segment 


Figure  23.  Non-Convex  Intercept 
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as  a  midpoint)  to  see  if  it  is  included  inside  the  original 
volume.  If  this  boolean  test  is  true,  the  new  edge  is 
"legitimate."  If  the  test  is  false,  then  the  new  edge  is 
outside  the  original  volume,  and  must  be  discarded.  Only  one 
point  needs  to  be  tested  for  inclusion  in  the  original 
volume,  since  the  unwanted  edges  lie  entirely  outside  the 
original  volume  (except  for  their  endpoints,  of  course). 

The  non-convex  interception  case  only  occurs 
during  an  early  part  of  the  static  visibility-determination 
phase,  and  then  only  during  a  clearly  identifiable  portion  of 
the  algorithm.  This  is  discussed  in  more  detail  in  the 
following  chapter. 

2.  Facet  Detection 

Recall  that  a  facet  is  composed  of  a  set  of  planar 
points  connected  in  a  cycle  by  edges.  The  intersection 
algorithm  places  points  and  edges  into  the  appropriate 
resultant  volumes,  but  no  facet  information  is  passed  from 
the  original  volume  to  the  resultant  volumes.  The  first  of 
the  two  facet  finding  methods  presented  here  needs  only  this 
information  to  find  all  of  the  facets  of  the  resultant 
volumes;  the  algorithm  searches  through  the  edges  of  each 
resultant  volume  to  find  all  facets.  This  is  the  search- 
based  facet  finding  method.  The  second  method  presented 
cheats  a  little,  and  uses  some  information  from  the  original 
volume  to  help  find  the  facets  of  the  resultant  volumes  after 
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an  interception.  This  is  the  plane-based  facet  finding 
method. 

a.  Search-Based  Facet  Finding  Method 

This  algorithm  uses  a  best-first  search  to  find 
all  the  facets  of  a  volume.  Recall  that  a  best-first  search 
is  an  A*  search  which  uses  only  an  evaluation  function.  This 
facet-determination  search  finds  a  plane  in  a  volume  defined 
by  two  edges,  and  then  attempts  to  build  the  shortest 
possible  coplanar  closed  loop  of  edges  back  to  a  designated 
endpoint  of  one  of  the  lines. 

The  search  is  initialized  by  choosing  any  point 
as  a  start  point,  and  then  finding  two  connected  edges  which 
have  the  start  point  as  an  endpoint  (as  edges  LI  and  L2  of 
Figure  24).  New  edges  are  added  to  this  initial  set  of  lines 
if  and  only  if: 

•  The  new  edge  is  connected  to  the  end  of  the  current 
edge,  and 

•  The  new  edge  is  in  the  same  plane  as  the  initial  two 
edges,  and 

•  The  opposite  endpoint  of  the  new  edge  is  closer  to  the 
start  point  than  any  other  edge  which  meets  the  criteria 
above.  This  causes  the  search  to  loop  back  towards  the 
start  point,  creating  the  needed  cycle  of  edges. 

In  the  volume  of  Figure  24,  for  example,  the 
search  will  produce  a  facet  consisting  of  edges  LI,  L2,  L4, 
L6,  L8  and  L9 ,  all  endpoints  of  those  edges,  and  the  plane 
equation  of  the  shaded  plane. 


60 


b.  The  Plane-Based  Facet  Finding  Method 

This  method  relies  on  the  fact  that  all  of  the 
facet  plane  equations  of  the  original  volume  are  known,  as  is 
the  plane  equation  of  the  intersecting  plane.  Since  the 
resultant  volumes  are  made  from  the  elements  of  the  original 
volume,  all  of  the  facets  of  the  resultant  volumes  will  have 
plane  equations  equal  to  those  of  the  original  volume,  plus 
one  with  the  plane  equation  of  the  intersecting  plane.  Some 
of  the  original  volumes'  plane  equations  may  appear  in  only 
one  resultant  volume,  and  some  will  appear  in  both  resultant 
volumes . 

As  a  result,  to  find  all  of  the  facets  of  the 
resultant  volumes,  points  and  edges  need  only  be  partitioned 
into  the  plane  equations  in  which  they  lie.  Points  and  edges 
will  lie  in  only  two  plane  equations.  Facets  may  then  be 
determined  relatively  quickly  and  easily. 

This  method  requires  that  the  original  volume  and 
resultant  volumes  be  convex,  however,  since  no  attempt  is 
made  to  establish  the  connectivity  of  line  segments  in  the 
facets  created;  this  is  assumed  to  be  true. 

c.  Discussion 

As  can  be  inferred  from  the  above  description, 
the  facet  determination  methods  each  have  certain  cases  in 
which  they  are  best  used,  and  others  in  which  they  are  best 
avoided.  In  general,  the  search-based  method  is  suitable  for 
most  types  of  non-convex  volumes,  and  when  inputting  terrain 
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data.  The  plane-based  approach  is  best  used  for  all  types  of 
convex  volumes  where  some  previous  information  is  available. 
Both  algorithms  were  implemented.  The  search-based  method  is 
used  when  dealing  with  non-convex  volumes  and  after  certain 
error  conditions.  The  plane-based  method  is  used  for  all 
convex  volumes. 
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IV.  IMPLEMENTATION 


The  algorithms  described  in  the  previous  chapter  were 
implemented  in  LISP.  LISP  was  selected  due  to  the 
availability  of  LISP  machines  and  advantages  in  speed, 
numerical  accuracy  and  sophisticated  data-structure 
management . 

A.  SYSTEM  SPECIFICATIONS  AND  REQUIREMENTS 

The  program  was  implemented  on  a  Texas  Instruments  (TI) 
Explorer  II  LISP  machine  with  16  megabytes  of  memory,  60 
megabytes  of  virtual  memory,  using  Common  LISP  with  the  LISP 
Flavor  System  (version  6.0).  The  LISP  code  will  also  run  on 
a  TI  Explorer  I  LISP  machine  with  significant  increases  in 
execution  times.  The  LISP  code  should  also  run  on  any 
Symbolics  LISP  machine  with  only  minor  changes  to  the  flavor 
declarations  and  message  passing  formats,  although  this  has 
not  been  tried. 

The  LISP  implementation  is  in  the  seven  files  shown  in 
Table  2.  All  primary  data  structures  were  written  using  the 
LISP  Flavor  System,  an  object-oriented  programming 
environment.  Some  minor  data  types  were  implemented  as 
property  lists.  The  uncompiled  code  (interpretable)  in  the 
seven  files  occupies  about  150,000  bytes  {150K)  of  memory, 
and  the  compiled  version  occupies  approximately  140K  of 
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TABLE  2.  DISK  FILES  FOR  THREE-DIMENSIONAL  PATH  PLANNING 
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Other  functions  in  the 
implementation. 


TABLE  2  (CONTINUED) 


u 
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1.  Terrain  Input  Requirements 


Two  alternate  terrain  input  data  formats  have  been 
implemented.  The  first  method,  accepts  terrain  data  as  a 
series  of  disjoint  edges  defined  by  endpoints.  All 
structural  lines  shown  in  Figure  25  (and  defined  in  Chapter 
III)  must  be  included  in  the  input  data.  The  second  input 
format  accepts  terrain  data  as  a  series  of  disjoint  facets 
defined  by  the  vertices  of  the  facet.  This  input  method  will 
create  the  structural  lines,  with  certain  restrictions. 

The  first  input  method  is  designed  as  a  simple,  fast 
method  of  entering  terrain  data.  Input  is  volume-oriented, 
and  only  two  volumes  may  be  entered:  one  ground  volume  and 
one  air  volume.  The  input  format  is  shown  in  Table  3. 
Certain  non-convex  terrain  features  may  not  be  entered  using 
this  format,  including  peaks  and  other  embedded  terrain 
features  (see  Figure  26). 

The  second  input  method  is  much  more  general  than  the 
first,  and  is  what  "real"  polyhedral  input  terrain  would  be 
like.  The  method  inputs  only  the  terrain  itself,  builds  all 
structural  lines,  and  creates  the  air  and  ground  volumes. 
Terrain  may  be  input  in  multiple  rectangular  segments,  which 
allows  for  entering  the  embedded  terrain  features  of  Figure 
26.  The  only  restrictions  on  the  terrain  data  are  that  the 
terrain  segments  must  be  rectangular  with  sides  parallel  to 
the  x-axis  and  y-axis.  Table  4  provides  an  example  of  the 
data  format  for  this  input  method. 
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Air-Volume  Structural  Lines 


Ground-Volume  Structural  Lines 


Figure  25.  Terrain  Structure 
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Neither  input  method  is  fault  tolerant.  Data  is 
assumed  to  be  correct,  with  no  missing  facets,  points  or 
edges.  Sample  terrain  data  for  both  formats  is  included  in 
Appendix  A. 

2 .  Output  Formats 

Two  outputs  are  produced  by  the  implementation,  text 
and  graphical.  The  text  output  shows  the  state  of  execution, 
while  the  graphical  displays  show  the  results  of 
computations . 

Graphic  displays  are  two-dimensional  true-perspective 
projections  of  wire-frame  images  of  various  terrain,  volume 
or  path  features.  No  shading  or  hidden-line  algorithms  are 
used.  The  simple  graphics  flavors  used  were  not  written  as  a 
part  of  this  thesis  [Ref.  16].  Various  fixed  viewpoints  can 
be  used  to  display  objects  in  one  display  window.  Multiple 
display  windows  are  shown  at  once.  Alphanumeric 

representations  showing  locations  of  observers,  start  and 
goal  points. 

Graphic  displays  include: 

•  (DISPLAY):  Display  all  volumes. 

•  (DISPLAY-VOLUMES  {List}):  Display  selected  volumes. 

•  (DISPLAY-VISIBLE  {Observer});  Display  volumes  visible 
from  a  given  observer. 

•  (DISPLAY-NOT-VISIBLE  {Observer}):  Display  volumes  not 
visible  from  a  given  observer. 

•  (DISPLAY-PATH  {Path}):  Display  a  given  path  and  all 
ground  volumes. 
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•  (DISPLAY- PATHS  {List}):  Display  selected  paths  and  all 
ground  volumes . 

Examples  of  most  of  these  displays  are  shown  in 
Chapter  V. 

B.  DATA  STRUCTURES 

The  data  structures  used  in  the  three-dimensional  path 
planning  program  closely  mirror  the  basic  geometric  concepts 
presented  in  Chapter  II.  Information  relative  to  path 
planning,  visibility  and  other  important  parameters  has  also 
been  added. 

The  LISP  Flavor  System  defines  templates  for  data 
structures  (called  flavors),  which  can  then  be  created 
(instantiated)  with  unique  names  and  components  (called 
instantiation  values).  LISP  flavors  and  instantiation  values 
are  equivalent  to  frames  and  slots  as  defined  in  [Ref.  2J  and 
[Ref.  17].  These  instantiations  (or  frames)  can  represent 
points,  vectors,  edges,  facets,  planes,  volumes,  paths  or 
other  objects.  All  major  data  structures  in  this 
implementation  were  written  using  flavors. 

Volumes  and  paths  are  the  basic  units  of  manipulation  in 
the  three-dimensional  path  planning  program,  but  a  higher 
level  exists.  That  higher  level  is  the  "universe,"  the 
collection  of  all  observers,  volumes  (ground  and  air) 
relative  to  a  particular  terrain  model.  As  the  visibility 
determination  progresses,  all  the  changes  in  volumes  are  made 
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within  the  context  of  this  universe  flavor.  The  names  of  all 
paths  are  kept  in  a  single  global  variable  *LIST-OF-PATHS* . 

Volumes,  as  discussed  in  Chapter  II,  are  composed  of 
points,  edges  and  facets,  as  well  as  many  other  data  items. 
Table  5  shows  all  of  the  instantiation  values  (slots)  in  the 
volume  flavor.  Similarly,  each  of  the  main  constituent  parts 
of  a  volume  are  themselves  flavors,  and  have  other,  unique 
data  structures,  as  shown  in  Table  6,  As  described  in 
Chapter  II,  edges  are  composed  of  vectors,  this  flavor  is 
also  described  in  Table  6, 

The  remaining  flavors  used  are  for  observers  and  agenda- 
items  (for  the  A*  search).  Details  concerning  these  flavors 
may  be  found  in  Appendix  B. 

C .  PROGRAM  STRUCTURE 

The  path-planning  program  presented  here  has  a  sequential 
structure  which  follows  from  the  algorithms  presented  in 
Chapter  III.  an  annotated  black-box  control  diagram  is  shown 
in  Figure  27. 

1.  Visibility-Model  Functional  Description 

Functions  SET-UP  and  SET-UP-2  implement  the  set  of 
static-visibility  determination  algorithms  described  in 
Chapter  III.  other  than  starting  these  functions  by  calling 
them,  and  entering  some  observer  data  between  the  functions, 
little  other  user  intervention  is  required.  Upon  completion 
of  the  visibility  determination  phase,  the  user  may  use  the 
DISPLAY  or  DISPLAY-VOLUMES  function  to  verify  the  results. 
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TABLE  5.  SLOTS  IN  THE  VOLUME  FLAVOR 


Mixin  Flavor;  Graphic  (used  for  display  of  the  volume).  See 

Appendix  B. 


Function;  SET-UP 


Figure  27.  Block  Diagram  of  Program  Structure 
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Several  global  variables  are  used  to  save  intermediate  states 
of  the  static-visibility  determination  process.  These 
variables  are  shown  in  Table  7. 

a.  Input  Terrain  Data 

All  constants  are  initialized  and  global 
variables  reset  when  function  SET-UP  is  called.  The  terrain 
data  is  then  read  from  a  user-specified  disk  file  using  a 
user-specified  input  method.  Figure  28a  illustrates  output. 

b.  Finding  Ridges 

Function  SET-UP  continues  by  calling  function 
FIND-ALL-RIDGES,  which  tests  all  edges  in  the  ground  volume 
for  their  status  as  ridge  lines.  The  test  results  are  put  in 
the  data  structure  representing  the  edge.  Figure  28b  shows 
sample  text  output. 

c .  Making  Air  Volumes  Convex 

Function  SET-UP  continues  by  calling  function 
MAKE-CONVEX-VOLUMES.  This  makes  vertical  planes  through  each 
ridge  line,  and  then  intersects  all  air  volumes  with  this 
plane.  This  produces  convex  air  volumes  from  non-convex  air 
volumes  as  shown  in  Figure  29.  This  is  the  only  time  that 
non-convex  intersection  can  occur,  and  the  more  restrictive 
intersection  requirements  for  non-convex  volumes  are  enabled 
here,  as  described  in  Chapter  III,  Section  Cl-c.  Figure  28c 
shows  sample  output. 
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TABLE  7.  GLOBAL  VARIABLES 


Variable  Name 
♦ORIGINAL-INPUT-VOLUMES 

♦CON  VEX- VOLUMES* 

♦FINAL-VISIBILITY-REGIONS* 


Contents 

'  The  names  of  input  volumes 
before  manipulations. 

The  names  of  volumes  after 
the  air  volumes  were  made 
convex. 

The  names  of  all  visibility 
volumes  after  visibility 
determination. 
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>>>>  Volume  created:  |volume0002|  Composition:  GROUND 

>»>  Volume  created:  |volume0003|  Composition:  AIR 

(a) 


Find  all  ridges  in  ground  terrain: 

Ridge  check,  line:  |line0010| 

Ridge  check,  line:  |line0009| 

Ridge  check,  line:  |line0008|  —  Ridge 
Ridge  check,  line:  (line0007| 

(b) 


Making  air  volumes  convex: 

Air  volumes:  ( ( | volumeOOll | )  ( | volumeOOlS | ) ) 

Ridge  planes;  (|plane0113|  |plane0114|  |plane0116|  |plane0117|) 

intersecting:  ( | volumeOOl 4 |  (1/2000  1/1000  0  1))  -  Result:  ( | facet0071 | ) 

intersecting:  <( volumeOOl 1 t  <1/2000  1/1000  0  1) )  -  Result:  nil  (late  1) 

intersecting:  ( 1 volumeOOll |  (1  -2  0  0))  -  Result:  ( | f acet0075 I ) 

Intersecting:  (( volumeOOl 9 (  (1  -2  0  0))  -  Result:  nil  (late  1) 

intersecting;  ( |  volun\e001 8 1  (1  -2  0  0)) - Result:  nil  (late  1) 

(c) 


making  visibility  regions  for:  | ob3erver0002 | 

Air  voliimes:  (  ( |  volumeOOOS  |  )  ( |  volume0004  |  )  ) 

Limiting  planes  of  visibility;  ( |plane0018 | ) 

intersecting:  ( | voliimeOOOS |  (-1/750  0  1/300  1>) - Result;  ( | facet 0023 | ) 

intersecting:  ( | volume0004 |  (-1/750  0  1/300  1))  -  Result:  ( | f acet0028 | ) 

(d) 


Figure  28.  Text  output 
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Non-Convex  Air  Volume 
(a) 


Figure  29.  Making  Air  Volumes  Convex 


d.  Enter  Observer  Data 

This  is  a  manual  user  step  to  enter  the  location 
and  effectiveness  of  observers  to  be  used  in  the  visibility 
model.  This  is  done  using  function  INIT-OBSERVER .  Location 
is  entered  as  an  (x,y,2>  coordinate,  and  observer 
effectiveness  is  entered  as  a  decimal  probability  of 
detection  between  0.0  (no  probability  of  detection)  and  1.0 
(100%  probability  of  detection).  An  observer  flavor  is 
instantiated  by  this  function,  and  added  to  the  universe 
instantiation . 

e.  Creating  Visibility  Volumes 

A  call  to  function  SET-UP-2  will  automate  the 
rest  of  the  static-visibility  determination.  This  function 
will  first  call  function  MAKE-VISIBILITY-REGION  for  each 
observer  entered  by  the  user.  This  completes  the  visibility 
determination  algorithm  described  in  Chapter  III.  Figure  28d 
illustrates  textual  output  from  this  function. 

f.  Finding  Visibility 

SET-UP-2  then  calls  function  DETERMINE-VISIBILITY 
for  each  observer.  This  function  determines  the  line-of- 
sight  (LOS)  from  the  center  of  each  visibility  volume  to  the 
observer.  If  the  LOS  is  not  blocked,  the  name  of  the 
observer  is  added  to  a  list  of  observer  names  in  the  volume 
data  structure.  Figure  30a  shows  a  sample  output. 

Function  SET-UP-2  then  calls  function 
PROBABILITIES-ASSUMING-INDEPENDENCE-OR  [Ref.  2],  which 
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visibility  determination  for:  | observer0002 | 


|volume0007|  visible 
|volume0009|  not  visible 
|volume0008|  visible 

(a) 


Determine  Probability  of  Detection  for  visibility  volume 


I  volumeOOOS 1 
I  volume0009 | 
1  volume0002  | 


has  P.D. :  0.75 
has  P .D  .  :  0.0 
has  P . D . :  0.0 


(b) 


Connecting  volumes: 


volume0006 | 

Connected 

to 

:  ( 1  volunie0007  | 

1  volumeOOOB 

volume0007 ( 

Connected 

to 

:  ( 1 volume0006 | ) 

volumeOOOS | 

Connected 

to 

:  ( 1 volume0009 1 

1  volumeOOOe 

volume0009 | 

Connected 

to 

;  ( 1 volumeOOOS | ) 

volume0002 | 

Connected 

to 

:  NIL 

(c) 

Figure  : 

30, 

Text  Output 
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combines  the  probabilities  of  detection  for  all  observers 


which  can  see  a  given  visibility  volume  (using  the 

information  noted  earlier  in  the  data  structure  of  the 
visibility  volume).  Figure  30b  shows  a  sample  output, 
g.  Connecting  Voluaes 

Finally,  SET-UP-2  calls  function  CONNECT-VOLUMES . 
This  function  builds  the  search  graph  by  annotating  the  data 
structure  of  each  visibility  volume  with  the  names  of  the 
visibility  volumes  to  which  it  is  adjacent,  using  the 
algorithm  described  in  Chapter  III,  Section  A2-c.  figure  30c 
shows  a  sample  output. 

2.  Path  Planning 

The  path-planning  phase  is  less  structured  than  the 
visibility  determination  phase,  so  no  overall  control 
function  has  been  implemented.  The  user  must  follow  a 
minimum  sequence  of  actions,  as  shown  below, 
a.  Entering  Start  and  Goal  Points 

These  points  are  entered  using  function  INIT- 

POINT. 


b.  Volume  Path  Planning 

Function  A-STAR-SEARCH  performs  A*  search  for  the 
volume  path  through  the  visibility  volumes.  This  function 
will  instantiate  a  unique  name  for  the  path  created,  make  the 
initial  guess  at  the  piecewise-linear  path,  and  place  that 
information  into  the  paths  data  structure. 
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An  alternate  function,  A-STAR-SEARCH-M,  finds 
multiple  volume  paths  from  the  start  point  to  the  goal  point. 
The  paths  may  be  compared  at  a  later  time, 
c.  Optimizing  the  Path 

Finally,  the  user  may  call  function  OPTIMIZE-PAIH 
which  will  conduct  one  Snell's  Law  optimization  pass  along  a 
given  volume  path.  This  optimization  function  will  create  a 
new  instantiation  of  the  path  flavor  for  the  optimized  path, 
the  user  may  call  this  function  repeatedly  to  obtain  the 
effect  of  multiple  optimization  passes.  New  paths  are 
created  on  each  call  to  let  the  user  compare  paths. 

D.  VOLUME  INTERCEPT  FUNCTIONS 

The  functions  which  subdivide  a  volume  by  a  plane,  while 
not  directly  called  by  the  user,  form  the  heart  of  the 
visibility  determination  algorithm.  MAKE-CONVEX-VOLUMES  and 
DETERMINE-VISIBILITY  are  the  functions  which  directly  call 
the  series  of  volume  intercept  functions.  These  user-level 
intercept  functions  develop  lists  of  volumes  to  be  subdivided 
and  lists  of  intercepting  planes.  The  highest  level 

intercept  function,  INTERSECT-ALL-PLANES-WITH-VOLUMES 

conducts  repeated  intercepts  of  the  volumes  with  the 
intercepting  planes.  The  resultant  volumes  replace  the 
original  volume  in  the  list  of  volumes  to  be  intercepted. 

The  volume  intercepting  algorithm  described  in  Chapter 
III  is  implemented  by  the  lowest  level  intercept  function 
INTERSECT.  This  function  takes  onf  volume  and  one  plane,  and 
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performs  their  intercept.  Text  output  from  this  function  is 
of  the  format: 

Intercepting:  {volume-name}  {ABC  Ao}  -  Result: 

{result-code} 

where  A,  B,  C  and  Ao  are  the  constant  coefficients  of  the 
intercepting  plane.  Figures  28c  and  28d  show  examples  of 
this  output.  Possible  result  codes  are  shown  in  Table  8. 

Numeric  errors  can  occur  during  the  intercept  function. 
They  are  the  result  of  a  rounding  error  associated  with  the 
use  of  floating  point  numbers  in  the  calculation  of  point 
values  and  various  coefficient  values  for  vectors,  edges  and 
planes.  This  results  in  points  which  do  not  lie  in  planes 
that  they  should,  lines  that  do  not  connect  though  they 
should,  vectors  that  are  equal  being  found  not  equal,  etc. 
When  a  numeric  error  occurs,  the  intercept  function  will 
simply  indicate  that  no  intercept  has  occurred,  a  degenerate 
intercept  condition  is  produced,  and  execution  is  not  halted. 
The  intercepting  plane  which  caused  the  error  is  flagged  for 
later  use. 

A  method  was  developed  to  attempt  to  correct  these 
numeric  errors  when  they  occur.  This  method  adjusts  the 
allowed  imprecision  of  calculations,  broadening  the 
definition  of  equality  between  numbers  [Ref.  18]. 

A=B  if  and  only  if  (A  -  B)/B  <  {fixed  constant}  (4-1) 
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TABLE  8.  INTERCEPT  RESULT-CODES 


Code 

nil  (earlyl) 
nil  (early2) 

nil  (late  1) 
nil  (late  2) 


({facet-name}) 


({facet-name-l ) 


Violated  Euler's 


Description 

Invalid  intercepting  plane. 

Intercept  along  a  facet  of  a 
convex  volume. 

Intercept  at  a  point  (only). 

Not  enough  facets  in  resultant 
volume.  Usually  indicates  that  a 
numeric  error  occurred. 

Successful  intercept,  facet 

named  is  the  common  facet  between 

the  resultant  volumes. 

{facet-name-2) ...) 

Successful  intercept,  except  that, 
due  to  a  numeric  error,  several 
facets  are  common  to  the  resultant 
volumes. 

Relation  #.## 

A  numeric  error  occurred 
at  precision  #.##. 
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The  value  of  the  fixed  constant  value  is  increased 
incrementally  to  fix  a  numeric  error.  The  normal  fixed 
constant  is  initially  0.25%.  While  correcting  an  error  this 
will  be  increased  incrementally  up  to  a  maximum  of  1.0%. 

If  the  correction  method  is  unsuccessful  in  correcting 
the  numeric  error,  then  no  further  action  is  taken.  This  can 
have  a  significant  impact  on  the  validity  of  the  visibility 
model,  and  is  discussed  further  in  Chapter  VI. 
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V .  RESULTS 


Two  useful  metrics  were  developed  to  measure  the 
effectiveness  of  the  program.  The  number  of  visibility 
volumes  built  is  compared  to  the  number  of  visibility  cubes 
created  in  a  uniform-cube  approach  to  the  visibility  problem 
to  measure  the  effectiveness  of  the  visibility-determination 
phase.  The  weighted  average  probability-of-detection  for  the 
path  is  compared  to  the  maximum  probability-of-detection  for 
the  path  to  measure  the  effectiveness  of  the  path  planning. 

A.  VISIBILITY  VOLUMES 

Six  different  airspace  models  were  tested  in  the 
visibility-determination  phase,  each  in  three  different 
situations,  for  a  total  of  18  test  cases.  Twelve  of  these 
test  cases  cover  airspace  over  simple  terrain  models  with 
isolated,  idealized  terrain  features.  These  include  various 
simple  ridge  and  peak  shapes.  The  remaining  test  cases 
represent  more  complex  terrain.  All  terrain  was  bounded 
inside  a  1000  by  1000  by  1000  cube  (of  no  units). 

Airspace  models  were  tested  after  air  volumes  were  made 
convex,  after  visibility  processing  for  one  observer,  and 
after  processing  for  two  observers.  The  number  of  visibility 
volumes  was  recorded  after  each  case.  The  effectiveness  of 
the  visibility  model  was  judged  against  the  reduction  in  size 
of  the  search  graph,  relative  to  a  standard  maximal  search 
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graph.  This  standard  maximal  search  graph  is  composed  of  10 
by  10  by  10  cubes  (800,000  nodes),  or  25  by  25  by  25  cubes 
(52,100  nodes),  or  50  by  50  by  50  cubes  (6,400  nodes).  These 
assume  an  initial  air  volume  from  ground  level  at  200  units 
to  a  maximum  altitude  of  1000  units.  The  presence  of  terrain 
features  represents  a  25%  decrease  in  the  size  of  the  air 
volume,  so  the  final  sizes  of  the  standard  maximal  search 
graphs  are  600,000,  38,400  and  4,800  nodes,  respectively. 

Table  9  shows  the  rtsnlts  obtained  for  the  airspace 
models.  Figure  31  shows  one  of  these  models  (a  simple  ridge, 
called  a  "single  ridge"  in  Table  9)  after  the  air  volumes 
have  been  made  convex.  Figure  32  illustrates  the  same  model 
after  processing  for  a  single  observer  (the  location  of  the 
observer  is  noted  with  a  lozenged  OBS  symbol).  Figure  33  and 
34  show  the  same  views  with  another  terrain  model  (a  complex 
ridge  structure  called  a  "full  ridge"  in  Table  9)  with  two 
observers.  It  should  be  clear  from  Figure  34  that  a 
graphical  display  of  the  visibility  regions  in  even  a  simple 
problem  is  too  confusing  to  be  useful. 

Table  9  indicates  that  the  number  of  visibility  volumes 
grows  explosively  as  a  function  of  the  number  of  ridge  lines 
and  the  number  of  observers.  The  graphs  of  Figure  35  clearly 
indicate  a  strong  functional  relationship  between  these 
quantities.  The  growth  in  the  numbers  of  visibility  volumes 
approximates  an  exponential  curve,  as  a  function  of  the 
number  of  ridge  lines,  as  shown  in  Figure  35b.  Such  growth 
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Figure  32.  Visibility  Volumes  with  One  Observer  over  a 
Single  Ridge 
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Figure  33.  Convex  Air  Volumes  over  a  Full  Ridge 
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Figure  34.  Visibility  Volumes  for  a  Full  Ridge  with  Two 
Observers 


95 


NumlMr  al  VlilblUly  Vakimaa  (On*  ObawvM)  Numbw  ol  VlitbIMy  VokmiM  (Ona  ObsMVw) 


Figure  35. 


Number  of  Ridge  Lines  vs  Visibility  Volumes 

yllt13*10*(022S3ic)  R.0g4 


Numbar  of  Rldgo  Lino* 

(a) 


Number  of  Ridge  Lines  vs  Visibility  Volumes 


\  10 


Numbor  of  Rfdgo  Unoo 

(b) 


Plots  of  Ridge  Lines  vs  Visibility  Volumes 


alr-vo1u«e# 


|top  vlgut  ground 


IZX) 


ground 


Figure  31.  Convex  Air  Volumes  over 
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is  relatively  innocuous  with  simple  terrain  models,  but  with 
more  complex  models  this  growth  hurts  the  overall  performance 
of  the  path-planning  program.  This  is  discussed  in  more 
detail  in  the  following  chapter. 

Note  the  increased  incidence  of  numeric  errors  (as 
defined  in  Chapter  III,  Section  D)  encountered  as  the  number 
of  ridges  and  observers  increases,  as  shown  in  Table  9. 
These  errors  present  potential  deviations  from  the  ideal 
visibility  model.  Volumes  affected  by  numeric  errors  may  not 
have  uniform  visibility  for  all  enclosed  points. 

The  visibility  model  does  significantly  reduce  the  size 
of  the  search  graph  from  the  standard  maximal  graph,  as 
indicated  in  Table  10. 

B .  PATH  PLANNING 

Representative  results  for  the  path  planning  phase  are 
shown  in  Table  11  and  Table  12.  Figures  36  through  47 
illustrate  some  of  these  paths.  Three  views  are  shown  for 
each  terrain  model,  one  for  the  initial,  non-optimal 
piecewise-linear  path  (an  initial  guess,  based  on  the  volume 
path,  and  defined  in  Chapter  III,  Section  Bl-c),  one  for  the 
optimal  linear  path  (defined  in  Chapter  III,  Section  Bl-c), 
and  one  showing  both  paths  together. 

Figure  48  shows  the  effect  of  varying  the  location  of  the 
start  point  while  keeping  the  end  point  fixed.  This 
illustrates  the  trade-off  made  during  path  planning  between 
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TABLE  10.  REDUCTION  IN  SEARCH  GRAPH  SIZE 


Relative  size  of  search  graphs 
(visibility  model/standard  model) 
with  one  Observer 


One  Ridge 

0.0 

.0002 

.0005 

Two  Ridge 

0.0 

.0002 

.0015 

Peak 

0.0 

.0007 

.0050 

Full  Ridge 

0.0 

.0019 

.0140 

Box  Canyon 

.0002 

.0026 

.0203 

Double  Peak 

.0002 

.0024 

.0228 
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TABLE  11.  PATH  PLANNING  RESULTS  FOR  SIMPLE  TERRAIN 
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4th  optimize  1630 

5th  optimize  1615 

Final  optimize  1605 


Initernatt'Vleu 


Figure  36.  Initial  Piecewise-Linear  Path  over  a  Single  Ridge 
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Figure  38.  Combined  View  of  Paths  from  Figures  36  and  37 
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Figure  40.  Final  Piecewise-Linear  Path  for  a  Double  Ridge 
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Figure  41.  Combined  View  of  Paths  for  Figures  39  and  40 
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Figure  42.  Initial  Plecewise-Hnear  Path  over  a  Box  canyon 
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Figure  44.  Combined  View  of  Paths  from  Figures  42 


Figure  45.  Initial  Plecewlse-Linear  Path  over  a  Box  Canyon 
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Figure  46.  Final  Piecewise-Linear  Path  over  a  Box  Canyon 
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Figure  47.  Combined  View  of  Paths  for  Figures  45  and  46 
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Figure  48.  Paths  Found  with  Different  Start  Points  over 
a  Box  Canyon 


distance  travelled  and  probability  of  detection.  All  paths 
displayed  are  non-optimal  initial  guesses. 

Tables  11  and  12  also  show  the  effects  of  the  Snell's  Law 
optimization  (of  Chapter  III,  Section  Bl-c)  on  the  cost  and 
average  probability-of -detection  of  the  paths.  On  average,  a 
10%  reduction  in  total  cost  is  obtained  as  a  result  of 
optimization,  with  most  of  that  gain  being  realized  in  the 
first  optimization  step.  Figures  36  through  47  also  show 
that  the  optimized  paths  generally  remain  in  the  same  planes 
as  the  initial  path.  This  is  an  artifact  of  the  optimization 
procedure.  Lower-cost  optimal  paths  may  result  if  this 
restriction  is  removed. 
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VI .  CONCLUSIONS 


It  is  clear  from  the  previous  chapter  that  the 
visibility-volume  approach  presented  in  this  thesis  can  be 
successfully  applied  to  the  three-dimensional  path  planning 
problem.  Reasonable  paths  were  produced  over  a  variety  of 
terrain  types.  Search  graphs  were  reduced  in  size  compared 
to  the  alternate  approach  proposed  in  Chapter  V  (Section  A), 
usually  by  two  to  five  orders  of  magnitude.  The  search 
algorithm  used  was  an  easily  implemented  one,  and  no  exotic 
pruning  criteria  were  necessary  during  the  path-planning 
process . 

A .  KNOWN  PROBLEMS 

Several  shortcomings  of  this  approach  are  worth  noting, 
however.  The  growth  in  the  number  of  visibility  volumes  is 
exponential  as  the  number  of  ridge  lines  increases.  This 
slows  both  the  static-visibility  determination  and  the  path 
planning.  As  the  number  of  visibility  volumes  increases, 
their  average  size  decreases,  relative  to  the  size  of  the 
original  air  volumes.  Small  visibility  volumes  create  narrow 
volume  paths,  and  this  decreases  the  effectiveness  of  the 
optimizing  function  by  decreasing  the  area  available  to  move 
the  maneuver  points. 

The  presence  of  very  small  visibility  volumes  reduces  the 
chances  that  the  A*  search  agenda  will  ever  contain  areas  of 
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low  probability  of  detection.  If  A*  search  never  places  a 
low  probability-of-detection  volume  on  the  search  agenda,  it 
will  not  be  able  to  evaluate  the  effectiveness  of  planning  a 
path  through  that  volume. 

Occurrences  of  the  numeric  errors  discussed  earlier 
result  in  volumes  containing  regions  of  non-uniform 
visibility,  which  will  result  in  paths  being  planned  to  pass 
through  areas  of  high  visibility  in  preference  to  areas  of 
low  visibility.  Again,  the  extent  of  this  problem  was  not 
examined  in  this  thesis,  but  measures  recommended  to  correct 
other  shortcomings  will  help  to  alleviate  this  problem. 

No  true  aerodynamic  effects  have  been  included  in  the 
energy  model  proposed  in  this  thesis.  Such  models  exist,  but 
they  were  intentionally  left  out  to  avoid  over-complicating 
the  problem.  The  energy  model  used  here  predicts  energy  use 
in  the  volume  path-planning  stage,  and  only  minimizes 
distance  flown  and  visibility  during  the  path  optimization 
functions.  Optimization  must  be  extended  to  cover 
aerodynamic  effects  (turns,  climb  and  dive  effects).  Note 
that  this  will  require  extensions  to  Snell's  Law  to  account 
for  these  non-optical  effects. 

Not  until  an  optimal  piecewise-linear  path  is  found  is 
the  precise  nature  of  the  flight  path  known.  As  a  result, 
the  best  overall  path  may  be  through  a  non-optimal  volume 
path  (e.g.,  the  second  or  third  solution  found  by  the  A* 
search) .  The  nature  of  this  effect  should  be  investigated. 
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Non-point  observers,  such  as  a  small  group  of  radars  or 
dispersed  groups  of  men,  are  not  well  handled  by  the 
visibility  model.  Since  the  model  is  very  sensitive  to  an 
increase  in  the  number  of  observers  (which  cause  a  rapid  rise 
in  the  number  of  visibility  volumes,  and  a  corresponding 
decrease  in  the  size  of  the  individual  volumes  within  the 
fixed  airspace),  simulating  a  large  group  of  observers  will 
be  impossible  to  handle. 

No  diffraction,  refraction,  interference  or  diffusion 
effects  are  implemented.  These  can  be  significant  effects, 
especially  for  radar  and  sonar  observation.  Simple  range 
models  can  be  implemented  to  account  for  some  effects,  while 
empirically-based  changes  to  the  probability-of -detection  for 
certain  visibility  volumes  can  account  for  others. 
Diffraction  effects  due  to  terrain  could  be  approximated  by 
intersecting  volumes  with  curved  visibility-boundary  surfaces 
instead  of  planes.  Similar  approximations  can  be  made  to 
correct  lines-of-sight  for  various  atmospheric  effects. 

Finally,  the  program  is  not  fast.  Construction  of 
visibility  volumes  can  take  up  to  24  hours  to  complete  for 
complex  terrain  models  (for  the  double  peak  terrain  of  Table 
9  in  Chapter  V,  consisting  of  eight  ridge  lines  and  two 
observers).  Five  to  ten  hours  is  the  average  run  time  for  a 
simple  terrain  model  (the  peak  in  Table  9  in  Chapter  V, 
consisting  of  four  ridges  and  one  observer) .  Search  times 
are  very  short,  and  run  from  ten  seconds  to  several  minutes. 
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on  the  average;  large  numbers  of  visibility  volumes  will  take 
up  to  15  minutes. 

B .  RECOMMENDATIONS 

Corrective  measures  not  already  discussed  to  the  problems 
mentioned  above  fall  into  several  areas:  better  selection 
criteria  for  planes  of  visibility,  a  method  of  combining 
visibility  volumes,  parallel  processing  for  some  parts  of  the 
program,  and  modification  of  the  search  methodology  used. 

Algorithm  3-2  of  Chapter  III  for  the  selection  and  use  of 
limiting  planes  of  visibility  was  not  implemented  in  this 
thesis.  It  alone  will  not  prevent  rapid  growth  in  the  number 
of  visibility  volumes,  but  will  serve  to  slow  it  some.  That, 
combined  with  other  measures  (range  effects,  independent 
processing  of  terrain  segments,  etc.)  may  significantly 
control  the  number  of  visibility  volumes. 

A  second  corrective  measure,  coalescing  visibility 
volumes  after  their  creation  will  help  resolve  the  small- 
visibility-volume  problem.  This  would  involve  adjacent 
volumes  with  the  same  probability-of-detection .  This  could 
create  non-convex  visibility  volumes,  however,  and  must  be 
applied  with  caution. 

Much  of  the  visibility-determination  portion  of  the 
program  involves  intercepts  of  different  volumes  with  the 
same  intercepting  plane.  These  calculations  are  functionally 
independent  from  one  another,  and  thus  are  amenable  to  a 
parallel  processing.  Since  several  hundred  such 
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interceptions  occur,  significant  improvements  in  execution 
times  could  be  expected  with  the  use  of  several  parallel 
processors.  Parallel  processing  may  also  help  to  speed  up 
the  search  portion  of  the  program. 

The  two-phase  path-planning  methodology  used  here  should 
be  compared  with  a  single-phase  path-planning  algorithm. 
Finding  the  optimal  piecewise-linear  paths  as  the  A*  search 
progresses  may  result  in  finding  a  better  flight  path. 

C.  DISCUSSION 

There  are  two  advantages  to  the  volume-oriented  approach 
to  the  three-dimensional  path-planning  problem:  the 
abstraction  of  airspace  to  a  search  graph  based  on  large, 
irregular  visibility  volumes,  and  the  resultant 
simplification  of  the  search. 

Appropriate  use  of  the  visibility  volume  concept  ensures 
that  A*  search  can  effectively  evaluate  all  possible  paths 
from  the  start  point  to  the  goal  without  missing  large  areas 
of  the  search  graph.  As  mentioned  earlier.  A*  cannot 
evaluate  search  nodes  that  do  not  get  onto  the  search  agenda, 
and  the  use  of  large.  Irregularly  shaped  visibility  volumes 
ensures  that  the  largest  possible  amount  of  airspace  is 
represented  on  the  agenda  by  the  smallest  number  of  volumes. 
The  effect  of  small  vs.  large  versus  large  visibility  volumes 
on  the  effectiveness  of  A*  search  should  be  Investigated 
further . 
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Simplicity  of  the  search  function  is  the  second 
significant  advantage  of  this  approach  to  the  problem. 
Almost  all  of  the  work  necessary  to  do  a  search  is  done 
before  the  search  starts.  No  complex  (and  time  consuming) 
pruning  functions  are  implemented  during  the  search.  The 
only  work  not  done  before  the  search  is  the  actual 
calculation  of  the  cost  and  evaluation  functions. 
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APPENDIX  A.  SAMPLES 


This  appendix  contains  a  sample  program  run  for  a  single 
ridge  terrain  model  (see  Chapter  V,  Section  A)  with  one 
observer,  and  the  contents  of  two  terrain  data  files.  The 
program  run  shows  all  user  Inputs  and  program  responses  from 
initial  terrain  selection  through  path  planning  (with  a 
single  optimization  cycle). 
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Sample  run : 


>  (set-up  1  '"t20-ridge-x.llsp') 


*  Volume  Creation  and  Display  V1.1  * 


>Constants  Initialized 
>Camera  built 

>Universe  created;  reading  data  file 

»»  Volume  created:  (volume0002(  Composifion:  GROUND 
»»  Volume  created;  IvolumeOOOSj  Composition:  AIR 

>Creation  complete. 


Find  all  ridges  in  ground  terrain: 


Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line: 
Ridge  check,  line; 
Ridge  check,  line: 
Ridge  check,  line; 


|line0022| 

jiine002li 

|line0020| 

|line0019| 

|line0018| 

llineOOl?! 

|line0016| 

llineOOl  5| 

|line0014| 

llineOOl  3| 

llineOOl  2| 

jiineOOl  1 1 

llineOOl  0| 

|line0009| 

|line0008|-  Ridge 

|line0007| 

|line0006| 

|iine0005| 

|line0004| 

jlineOOOsj 

|line0002| 


Making  air  volumes  convex; 
Air  volumes:  ((|volume0003|)) 
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Ridge  planes:  (|plane0013|) 


intersecting:  (|volume0003|  (1/500  0  0  1))  ---  Result:  {|facet0017|) 
Re-doing  error  Intercepts:  NIL 


Enter  observer  data  now: 

NIL 

>  (init-observer  ’(0  500  300)  ’0.75) 

|observer0002| 

>  (set-up-2) 

- PRE-PROCESS  VISIBILITY  INFORMATION 


making  visibility  regions  for:  |observer0002| 

Air  volumes:  ((|volume0005|)  (|volume0004|)) 
Limiting  planes  of  visibility:  (|plane0018|) 


intersecting:  (|volume0005|  (-1/750  0  1/300  1))  ™  Result:  (|facet0023|) 
Intersecting:  (|volume0004j  (-1/750  0  1/300  1))  —  Result:  (ifacet0028i) 

Re-doing  error  intercepts:  NIL 


Numeric  errors:  NIL 


SPEED-DEMON-INVOKED 


Visibility  determination  for:  |observer0002| 

|volume0006|  visible 
ivolume0007i  visible 
jvolumeOOOOj  not  visible 
jvolumeOOOSI  visible 


Determine  Probability  of  Detection  for  visibility  volumes 
|volume0006|  has  P.D.:  0.75 
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|volunie0007|  has  P.D.:  0.75 
|volume0008|  has  P.D.:  0.75 
|volume0009|  has  P.D.:  0.0 
jvolume0002j  has  P.D.:  0.0 


'SPEED-DEMON-INVOKED 


Connecting  volumes: 

|volume0006|  Connected  to:  (|volume0007|  |volume0008|) 
|volume0007|  Connected  to:  (jvolumeOOOei) 

|volume0008|  Connected  to:  (|volume0009|  |volume0006|) 
|volume0009i  Connected  to:  (|volume0008i) 

|volume0002|  Connected  to:  NIL 


NIL 

>  (a-star-search  (init-point  ’(1000  1000  300))  (init-poinl  ’(0  0  300))  ’nil) 

»»Begin  A-star  Search 

Start  Volume:  |volume0009| 

Goal  Volume:  jvolumeOOOei 


Search.  Completed 

»»Path  Statistics: 

Maximum  detection  probability:  0.75 
Average  detection  probability:  0.4802796 
Total  length  of  path:  1764.1292 
Total  number  of  maneuvers:  2 

»»Path:  |path0002| 

|path0002| 

>  (optimize-path  ’|path0002|) 

Optimizing  path  |path0002|: 

Optimizing  at  facet  number  1  :  |facet0028| 
Optimizing  at  facet  number  2  :  |facet0017i 


Optimization  completed 
»»Path  Statistics: 
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Maximum  detection  probability:  0.75 
Average  detection  probability:  0.49770224 
Total  length  of  path:  1533.2666 
Total  number  of  maneuvers:  2 

»»Path:  |path0003| 

NIL 
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Sample  of  terrain  using  the  first  input  method: 


ridge-x.lisp:  a  ridge  oriented  paraiiei  to  the  y-axis  (aiong  a  plane  of  the  x-axis) 
USES  FIRST  INPUT  METHOD 
D.H. Lewis/Thesis 


((ground  nil)  ;  terrain 

((0  0  300)(0  1000  300)) 

((0  0  300)(300  0  300)) 

((300  0  300)(300  1 000  300)) 

((300  1000  300)(0  1000  300)) 

((300  1000  300)(500  1000  500)) 

((500  1000  500)(700  1000  300)) 

((500  1000  500)(500  0  500)) 

((500  0  500)(300  0  300)) 

((500  0  500)(700  0  300)) 

((700  0  300)(700  1000  300)) 

((700  0  300)(1000  0  300)) 

((1000  0  300)(1000  1000  300)) 

((1000  1000  300)(700  1000  300)) 

((0  0  0)(0  1 000  0))  ;  Structural  lines  for  ground  volume 

((0  0  0)(0  0  300)) 

((0  0  0)(1000  0  0)) 

((0  1000  0)(0  1000  300)) 

((1000  0  0)(1000  0  300)) 

((0  1000  0)(  1000  1000  0)) 

((1000  1000  0)(1000  1000  300)) 

((1000  1000  0)(1000  0  0))) 

((air  0.01)  :  terrain 

((0  0  300)(0  1000  300)) 

((0  0  300)(300  0  300)) 

((300  0  300)(300  1000  300)) 

((300  1000  300)(0  1000  300)) 

((300  1000  300)(500  1000  500)) 

((500  1000  500)(700  1000  300)) 

((500  1000  500)(500  0  500)) 

((500  0  500){300  0  300)) 

((500  0  500)(700  0  300)) 

((700  0  300)(700  1000  300)) 

((700  0  300)(1 000  0  300)) 

((1000  0  300)(1000  1000  300)) 

((1000  1000  300)(700  1000  300)) 
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;  structural  line  for  air  volume 


((0  0  1000)(0  1000  1000)) 

((0  0  1000)(0  0  300)) 

((0  0  1000)(1000  0  1000)) 

((0  1000  1000)(0  1000  300)) 

((1000  0  1000)(1 000  0  300)) 

((0  1000  1000)(1000  1000  1000)) 
((1000  1000  1000)(1000  1000  300)) 
((1000  1000  1000)(1000  0  1000))) 


Scimple  of  terrain  using  the  second  input  method 


Double  peak.lisp:  Two  adjacent  ideal  peaks 

USES  SECOND  INPUT  METHOD 

D.H.  Lewis  Thesis  11/12/88 


((((0  0  200) 

(500  250  500) 
(1000  0  200)) 

((0  0  200) 

(500  250  500) 

(0  500  200)) 

((0  500  200) 

(500  250  500) 
(1000  500  200)) 
((1000  500  200) 
(500  250  500) 
(1000  0  200))) 
(((0  500  200) 

(500  750  700) 
(1000  500  200)) 
((0  500  200) 

(500  750  700) 

(0  1000  200)) 

((0  1000  200) 

(500  750  700) 
(1000  1000  200)) 
((1000  1000  200) 
(500  750  700) 
(1000  500  200)))) 
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FILE  NAME:  Setup. lisp 


MAIN  CONTROL  FUNCTIONS 
This  file  contains  several  LISP  functions  perform  overall  control  of  the 
static  construction  phase  of  the  code.  They  include  the  initial  input  control  loops  (for 
both  input  methods,  and  the  control  loop  for  the  visibility  region 
construction  phase  of  the  static  construction.  The  initial  set-up 
functions  are  first,  followed  by  the  middle  phase  set-up  functions,  large 
scale  control  functions,  and  finally,  the  actual  input  methods  themselves. 

THESIS  D.  H.  LEWIS  20  OCT  88 


ROUTINE  TO  INPUT  A  DATA  STREAM  AND  CONSTRUCT  THE  VOLUME(S) 
THESIS/CS4452  D.H.  LEWIS  15  MAY  88 


Builds  the  standard  static  flavors  (Universe,  above,  below,  and  cameras), 
opens  and  reads  input  file,  and  carries  the  static  phase  through  making 
air  volumes  convex. 

MAIN  FUNCTIONS:  SET-UP  (METHOD  FILE) 

SET-UP-2 

OTHER  FUNCTIONS:  INITIALIZE-VOLUME 

MAKE-VOLUME-WITH-FACET-DATA 

DECREASSING-SORT-ON-X-P 

DECREASING-SORT-ON-Y-P 

PRINT-HEADER-1 


(defvar  'Universe*)  ;  location  of  names  for  all  flavors  used  in  static 
;  construction 

(defvar  'above*)  ;  standard  volumes  used  by  intercept  functions 

(defvar  'below')  ; 

(defvar  'input-stream')  ;  system  name  for  ron-standard  input  file 
(defvar  'output-stream')  ;  system  name  for  non-standard  output  file 
(defvar  'max-altitude*  TOGO)  ;  maximum  altitude  in  Input  Method  2 
(defvar  *min-altitude*  ’0)  ;  minimum  altitude  for  Input  Method  2 

(defvar  *not-convex-volumes*)  ;  flag  variable  for  Input  Method  2  which  tells 
;  which  facet  building  function(s)  to  use 

(defvar  'original-input-volumes*  ’nil)  ;  save  various  "states" 

(defvar  'convex-volumes*  ’nil) 

(defvar  'final-visibility-regions'  ’nil) 
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APPENDIX  B.  LISP  CODE  LISTING 


This  appendix  contains  the  listings  for  all  LISP  code  in 
the  files  shown  in  Table  2.  File  names  are  noted  at  the 
beginning  of  each  file  listing. 
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INITIAL  SETUP 


(defun  set-up  (Method  File) 

(print-header- 1) 

(make-origin)  ;  make  favorite  constants 

(make-null-vector) 

(setf  *above*  (make-instance  ’volume)) 

(setf  *below*  (make-instance  ’volume)) 

(setf  *not-convex-volunres*  ’t) 

(setf  *done-nud<ing-new-vislbility-volumes-flag*  ’nil) 

(setf  'precision*  ’0.0025) 

(setf  'large-integer*  ’1e4) 

(setf  *list-of-error-planes*  ’nil) 

(princ  ”>Constants  Initialized”)  (terpri) 

(make-cameras)  ;  create  camera 

(princ  ”>Camera  built”)  (terpri) 

(setf  'Universe*  (make-instance  'Universe  ;  create  universe  for  volumes 

rVolumes  ’() 

:observers  ’())) 

(princ  ”>Uni verse  created;  reading  data  file”)  (terpri)  (terpri) 

(setq  'input-stream*  (open  File  :direction  :input)) 

;  select  and  use  input  method 

(cond  ((equal  ’1  Method)  ;  volume  oriented  Input  method 

(do  ((data  (read  'input-stream*  nil  ’done) ;  read  all  volumes  into  universe 
(setf  data  (read  'Input-stream*  nil  ’done)))) 

((atom  data)) 

(push  (init-volume  data)  (universe-volumes  'Universe*)) 

(princ  ”»»  Volume  aeated;  ”) 

(print  (car  *list-of-volumes*)) 

(princ  ”  Composition; ") 

(print  (volume-composition  (eval  (car  *list-of- volumes*))))  (terpri) 
(make-all-facets  (car  *list-of-volumes*))) ;  make  all  facets  for  new  volume 
(loop  for  V  in  (universe-volumes  'universe') 
do  (setf  (volume-visibility  (eval  V))  ’nil)))  ;  remove  ambiant  visibility 

((equal  '2  Method)  ;  facet  oriented  Input  method 
(do  ((data  (read  'input-stream*  nil  ’done) ;  read  all  volumes  into  universe 
(setf  data  (read  'input-stream*  nil  ’done)))) 

((atom  data)) 

(loop  for  terrain-segment  in  data  ;  go  through  each  volume  segment 
do  (setf  (universe-volumes  'universe') 

(append  (make-volume-with-facet-data  terrain-segment) 
(universe-volumes  'universe*)))))) 

(t  (terpri)  (princ  'Enor:  Method  not  implemented”) 

(retum-from  set-up  ’Done))) 

(close  'input-stream*)  (terpri) 
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(princ  ''>Creation  complete.'')  (terpri)  (terpri) 

(setf  ‘original-input- volumes*  (universe-volumes  'universe*)) 

;  complete  Initial  setup  functions 


(find-all-ridges) 

(terpri) 

(make-convex-volumes) 

(setf  *not-convex-volumes*  ’nil) 

(setf  'convex-volumes*  (universe-volumes  'universe')) 

(terpri)  (terpri)  (princ  "Enter  observer  data  now: ')  (terpri)  (terpri)) 


(defun  print-header- 1  () 

(terpri) 

(terpri) 

(princ  "•••‘••••****»**«»****»»‘**»»******‘**»»»»»«»**‘“*****»«»*»**»»»**") 

(terpri) 

(princ  "*  Volume  Creation  and  Display  VI.  1  *") 

(terpri) 

(princ  "**********************************»*‘***‘*“*“*»*‘***»“**********") 
(terpri) 

(terpri)) 


-INPUT  METHOD  ONE 


(defun  Initialize-volume  (Volume  Data)  ;  expects  data  as: 

(cond  ;  (line  line  line  ....)  where  lines  are 

((null  Data)  Volume)  ;  (point  point)  where  points  are;  (x  y  z). 

:(((xyz)(xyz))  ((xyz)  (xyz))) 

(t  (create-new-line  Volume  (init-point  (caar  Data))  (init-point  (cadar  Data))) 
(Initialize-volume  Volume  (cdr  Data))))) 


(defun  create-new-line  (Volume  pti  pt2)  ;  put  points  and  lines  into  volume  instance 
(pushnew  pt1  (volume-points  (eval  Volume))) 

(pushnew  pt2  (volume-points  (eval  Volume))) 

(pushnew  (init-Line  (init-vector  ’'origin*  pt1)  (init-vector  pti  pt2)) 

(volume- Edges  (eval  Volume)))) 


— INPUT  METHOD  TWO - 


(defun  make-volume-with-facet-data  (data)  ;  construct  a  volume  from  a  formatted 
;  list  of  data  where  format  is: 

:  (facet  facet  facet...) 
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(let  ((terrain-facets  (build-terrain  data)) 

(terrain-box  (make-instance  ’bounding-box))  ;  used  to  find  limits  of  data 

;  points 

(ground-volume  (init-volume  (list  (list  'ground  ’nil)))) 

(air-volume  (init-volume  (list  (list  'air  ’nil)))) 

(points-and-iines  ’nil)) 

;  find  all  lines  and  points  in  terrain  facets 

(setf  points-and-lines  (info-on-facets  terrain-facets)) 

;  assign  values  to  air  and  ground  volumes 

(setf  (volume-composition  (evai  ground-volume))  ’ground)  ;  set  composition 
(setf  (volume-composition  (eval  air-volume))  ’air) 

(setf  (voiume-facets  (eval  ground-volume))  terrain-facets)  ;  put  terrain  facets 
(setf  (volume-facets  (eval  air-volume))  terrain-facets) 

;  construct  top/bottom  and  sides  of  ground  and  air  voiumes 

(send  terrain-box  :construct-bounding-box  (first  points-and-lines)) 

(let  ( (point- 1  (first  (find-point  (bounding-box-max-x  terrain-box)  ;comers  of  terrain 

(bounding-box-min-y  terrain-box) 

’nii 

(first  points-and-lines)))) 

(point-2  (first  (find-point  (bounding-box-max-x  terrain-box) 

(bounding-box-max-y  terrain-box) 

’nil 

(first  points-and-lines)))) 

(point-3  (first  (find-point  (bounding-box-min-x  terrain-box) 

(bounding-box-max-y  terrain-box) 

’nil 

(first  points-and-lines)))) 

(point-4  (first  (find-point  (bounding-box-rrWn-x  terrain-box) 

(bounding-box-min-y  terrain-box) 

’nil 

(first  points-and-lines)))) 

(points-41  (stabie-sort  (find-point  ’nil  ;  edges  of  terrain 

(bounding-box-min-y  terrain-box) 

'nil 

(first  points-and-lines)) 
#’decreaslng-soi1-x-p)) 

(points- 12  (stabie-sort  (find-point  (bounding-box-max-x  terrain-box) 

'nil 

’nil 

(first  points-and-lines)) 
#’decreaslng-sort-y-p)) 

(points-23  (stabie-sort  (find-point  ’nil 

(bounding-box-max-y  terrain-box) 

’nil 

(first  points-and-lines)) 
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#’deCT©aslng-sort-x-p)) 

(points-34  (stable-sort  (find-point  (bounding-box-mln-x  terrain-box) 

■nil 

•nil 

(first  points-and-lines)) 
rdeaeaslng-sort-y-p)) 

(top-points  ’nil)  ;  top  and  bottom 

(bottom-points  ’nil))  ;  points 

(loop  for  P  in  (list  point- 1  point-2  point-3  polnt-4)  ;  find  top  points 
do  (sett  top-points  (adjoin  (init-point  (list 

(first 

(send  (eval  P)  :llst-format)) 
(second 

(send  (eval  P)  :llst-format)) 
*max-altitude*)) 

top-points))) 

(setf  top-points  (reverse  top-points)) 

(setf  (voiume-facets  (eval  air- volume))  ;  make  top  facet 

(adjoin  (make-a-facet  top-points) 

(volume-facets  (eval  air-volume)))) 

(setf  (volume-facets  (eval  air-volume))  ;  make  top  sides 

(adjoin  (build-side-facet  (fourth  top-points)  ;  and  put  in  voiume 

(first  top-points) 
points-41) 

(volume-facets  (eval  air-volume)))) 

(setf  (volume-facets  (eval  air-volume)) 

(adjoin  (build-side-facet  (first  top-points) 

(second  top-points) 
points- 12) 

(volume-facets  (eval  air-volume)))) 

(setf  (volume-facets  (eval  air-volume)) 

(adjoin  (build-side-facet  (third  top-points) 

(second  top-points) 
points-23) 

(volume-facets  (eval  air-volume)))) 

(setf  (volume-facets  (eval  air-volume)) 

(adjoin  (build-side-facet  (four^  top-points) 

(third  top-points) 
point8-34) 

(volume-facets  (eval  air-volume)))) 

(loop  for  P  in  (list  point- 1  point-2  polnt-3  point-4)  ;  find  bottom  points 
do  (setf  bottom-points  (adjoin  (init-point  (list 

(first  (send  (eval  P)  ;list-format)) 
(second  (send  (eval  P)  ;list-format)) 
*min-altitude*)) 
bottom-points))) 

(setf  bottom-points  (reverse  bottom-points)) 

(setf  (volume-facets  (eval  ground-volume))  ;  make  bottom  facet 
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(adjoin  (make-a-facet  bottom-points) 

(voiume-facets  (evai  ground-voiume)))) 

(sett  (volume-facets  (evai  ground-volume))  ;  make  bottom  sides 

(adjoin  (build-side-facet  (fourth  bottom-points)  ;  and  put  in  volume 

(first  bottom-points) 
points-41) 

(volume-facets  (evai  ground-volume)))) 

(setf  (volume-facets  (evai  ground-volume)) 

(adjoin  (build-side-facet  (first  bottom-points) 

(secoTKl  bottom-points) 
points- 12) 

(volume-facets  (evai  ground-volume)))) 

(setf  (volume-facets  (evai  ground-volume)) 

(adjoin  (build-side-facet  (third  bottom-points) 

(second  bottom-points) 
points-23) 

(volume-facets  (evai  ground-volume)))) 

(setf  (volume-facets  (evai  ground-volume)) 

(adjoin  (bulld-side-facet  (fourth  bottom-points) 

(third  bottom-points) 
points-34) 

(volume-facets  (evai  ground-volume))))) 

;  complete  information  on  volumes 

(setf  points-and-lines  (Info-on-facets  (volume-facets  (evai  air-volume)))) 

(setf  (volume-points  (evai  air-volume))  (first  points-and-lines)) 

(setf  (volume-edges  (evai  air-volume))  (second  points-and-lines)) 

(setf  points-and-lines  (info-on-facets  (volume-facets  (evai  ground-volume)))) 
(setf  (volume-points  (evai  ground-volume))  (first  points-and-lines)) 

(setf  (volume-edges  (evai  ground-volume))  (second  points-and-lines)) 

(loop  for  V  in  (list  air-volume  ground-volume) 
do  (let  0 

(terpri)  (princ "»»  Volume  Created: ") 

(print  V)  (princ "  Composition: ')  (prini  (volume-composition  (evai  V))))) 
(list  air-volume  ground-volume))) 

(defun  decreasing-sort-x-p  (A  B) 

(cond  ((>  (first  (send  (evai  A)  :list-format)) 

(first  (sernl  (evai  B)  :list-format)))))) 

(defun  decreasing-sort-y-p  (A  B) 

(cond  ((>  (second  (send  (evai  A)  :list-format)) 

(second  (send  (evai  B)  :list-format)))))) 


COMPLETE  STATIC  PHASE 
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Functions  here  complete  the  static  phase  of  constniction  of  the  visibility 
regions. 

MAIN  FUNCTIONS;  SET-UP-2 


(defun  set-up-2  ()  ;  finish  inWai  setup  (after  observers  aeated) 

(let  ((observers  (universe-observers  ^universe*))) 

(terpri)  (terpri) 

(princ " - PRE-PROCESS  VISIBILITY  INFORMATION - •) 

(terpri)  (terpri) 

(loop  for  Obs  in  observers  ;divlde  up  universe  by  visibilities 

do  (make-visibility-regions  Obs)) 

(terpri)  (terpri) 

(princ  "Numeric  errors:  *)  (prini  "list-of-error-planes") 

(terpri)  (terpri) 

(send  "universe*  :save-static-items) 

(setf  "final-visibility-regions*  (universe-volumes  "universe")) 

(setf  "done-making-new-visibility-volumes-fiag"  ’t)  ;  speed  things  up 
(loop  for  Obs  in  observers  ;  find  who  can  see  what 

do  (speed-demon) 
do  (determlne-visibility  Obs)) 

(terpri)  (terpri) 

(princ  "Determine  Probability  of  Detection  for  visibility  volumes") 

(terpri) 

(loop  for  V  in  (universe-volumes  "universe")  ;  calc  prob  of  detection  for 
do  (probabilities-assuming-Independence-or  V)) ;  each  volume 
(terpri)  (terpri) 

(speed-demon) 

(connect-volumes)  ;  build  visibility  graph 

(terpri))) 
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FILE  NAME:  Volume-functions . lisp 
Mode:  LISP;  Syntax:  Common-lisp 

I 

:::  D.H.  Lewis  CS4452/THESIS  5May88 


FLAVORS  AND  METHODS 


FLAVOR:  . Point 


METHODS:  :LIST-FORMAT 
:LIST-FORMAT-REAL 
:LIST-FORMAT-4 
:PRINT 


;  give  the  x.y  and  z  values  as  a  three-tuple 
;  same,  in  real  number  format 
;  give  agraphics  4-tuple  "(x  y  z  1 )" 

;  print  info  on  point 


FLAVOR:  . Vector 


METHODS:  :LENQTH 
:UNIT-VECTOR 
:ENDPOINTS 
:LIST-FORMAT 
:LiST-FORMAT-REAL 
:PRINT 


;  calculate  length  of  vector 
;  return  the  coeff  of  the  unit  vector 
;  give  the  names  of  the  endpoints  of  the  vector 
;  give  the  coeffs  of  the  vector  as  a  3-tuple 
;  same,  execept  with  real  numbers 
;  print  coeff  values  to  output  file 


FLAVOR:  . Line-segment 


METHODS:  :ENDPOINTS  ;  Return  the  endpoints  of  the  line-segment 

:ENDPOiNT-LIST  ;  Return  endpoints  as  explicit  4-tuples 

:OTHER-END  (ENDPOINT)  ;  Given  one  endpoint,  return  the  other 
:START-POINT  ;  Return  the  Start  point  of  the  line-segment 

:END-POINT  ;  '  "  endpoint  ’  "  " 

: LENGTH  ;  Find  and  return  the  length  of  the 

line-segment 

:BACKSUBS  (T-LIST)  ;  SubsHute  the  (Tx  Ty  Tz)  list  into 

the  line  equation 

:MID-POINT  ;  Find  the  mid  point  of  the  line-segment 

:STRATTLE-PLANE-P  (PLANE)  ;  do  the  endpoints  of  the  line-segment 

lie  on  opposite  sides  of  the  plane? 


:PRINT 


FLAVOR:  . Plane 


METHODS:  :TEST-EQUAL  (PLANE)  ;  Do  two  planes  have  the  same  coeffs? 
:LIST-COEFF  ;  Ust  4-tuple  (X  Y  Z  Ao)  for  plane 

:LiST-COEFF-3  ;  List  3-tupie  (X  Y  Z)  for  plane 

:SUBS-POINT-INTO-PLANE  (POINT)  ;  Get  Ao  coeff  from  X,Y,Z  values  of  point 
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:  :FIND-Z-QIVEN-XY  (X  Y)  ;  Return  Z  value  of  point  in  plane 

:  ;FIND-Y-QIVEN-XZ  (X  Z)  ;  "  X . 

:  ;FIND-X-QIVEN-YZ  (Y  Z)  ;  *  Y . 

PRINT 

;  FLAVOR:  . Bounding-box 

;  METHODS:  :CONSTRUCT-BOUNDINQ-BOX  (POINTS)  ;  Build  a  3-D  limits  for  list  of 
points 

;  :INSIDE-BOUNDING-BOX  (POINT)  ;  Is  the  point  inside  the  limits? 

•FLAVOR:  . Facet 

•METHODS:  :POINTS 
;  :PRINT 

;  FLAVOR:  . Volume 

•METHODS:  :MAKE-EQUAL 
;  :CLEAR 

:  :FIND-ARITHMETIC-CENTER 

:  :MAKE-NODE-LIST 

:  :MAKE-POLYGON-LIST 

;  :PRINT 

•FLAVOR:  . Universe 

METHODS:  :SAVE-STATIC-rTEMS 
:REVERT-STATIC-ITEMS 


(defvar  *origin*) 

(defvar  *null-vector*) 

(defvar  *one-vector*  ’(1 .0  1 .0  1 .0  1 .0)) 

(defvar  *one-vector-3*  ’(1.0  1.0  1.0)) 

(defvar  'zero-vector*  ’(0.0  0.0  0.0  0.0)) 

(defvar  'zero-vector-O*  ’(0.0  0.0  0.0)) 

(defvar  *max-counter-value*  ’9999) 

(defvar  *done-making-new-visibility-volumes-flag*  ’nil) 

(defvar  *list-of-points*  ’nil) 

(defvar  *points-counter*  ’0) 

(defvar  *minimum-points-counter*  ’0) 

(defvar  *list-of-vectors*  ’nil) 

(defvar  ‘vectors-counter*  ’0) 

(defvar  'minimum-vectors-counter*  ’0) 

(defvar  *list-of-lines*  ’nil) 

(defvar  *lines-counter*  ’0) 

(defvar  *minimum-lines-counter‘  ’0) 
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(defvar  *list-of-planes‘  ’nil) 

(defvar  *planes-counter*  ’0) 

(defvar  *minimunvplanes-counter*  ’0) 

(defvar  *list-of-faoets*  ’()) 

(defvar  ‘facets-counter*  ’0) 

(defvar  *minimum-faoets-counter*  ’0) 

(defvar  *list-of-volunnes*  ’()) 

(defvar  ‘volumes-counter*  ’0) 

(defvar  'minimum-volunfies-counter*  ’0) 


; - POINT- — 

(defflavor  point 
(x-coord 
y-coord 
z-coord) 

0 

:gettable-instance-variables 

:settable-instance-variabies 

:inittable-instance-varlables 

routside-accessible-instance-variables) 

(defmethod  (point  :List-format)  ()  ;  return  a  3-tuple  "(X  Y  Z)" 

(list  x-coord  y-coord  z-coord)) 

(defmethod  (point ; List-format-real)  () ;  return  a  real  valued  3-tuple 
(map  ’list  ’*  (list  x-coord  y-coord  z-coord)  (make-list  3  :initial-element  ’1 .0))) 

(defmethod  (point  :List-format-4)  ()  ;  return  list  in  graphics  format 
(list  x-coord  y-coord  z-coord  ’1)) 

(defmethod  (point  :print)  () 

(pprint  (list  x-coord  y-coord  z-coord)  ‘output-stream*)) 


; - VECTOR - 

(defflavor  vector 
(i 
I 
K 

Start-point 

End-point) 

0 

:gettable-instance-variables 

:settable-instance-variables 
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:ininabfe-instanoe-vaiiables 

:outslde-accessible-instance-variables) 

(defmethod  (vector  Jength)  ()  ;  Calculate  the  length  of  a  vector 
(sqrt(abs(+  (Mi)  (*jj)  (*kk))))) 

(defmethod  (vector  :unit-vector)  ()  ;  make  a  unit  vector  from  a  vector 
(let  ((vector-length  (send  self  ;length))) 

(cond  ((equal-zero-p  vector-length)  ’(0.0  0.0  0.0)) 

(t  (map  ’list  ’/  (send  self  :list-format) 

(make-list  3  linitial-element  vector-length)))))) 

(defmethod  (vector  :endpoints)  ()  ;  find  the  endpoints  of  the  vector 
(list  Start-point  End-point)) 

(defmethod  (vector  :list-format)  ()  ;  return  the  values  of  the  vector  as  a  3-tuple 
(listijk)) 

(defmethod  (vector  :list-format-real)  ()  ;  return  a  real  valued  3-tuple 
(map  ’list  ’*  (list  i  j  k)  (make-list  3  rinitial-element  ’1.0))) 

(defmethod  (vector  :print)  () 

(pprint  (list  i  j  k  Start-point  End-point)  ‘output-stream*)) 


LINE  SEGMENT 


(detflavor  line-segment 

(t-max  : 

position-vector 
direction- vector 
characteristics) 

0 

:gettable-instance-variables 

rsettable-instance-variables 

:inittable-instance-variables 

:outside-accessible-instance- 


:  position  vector  can  point  to  either  end  of 
direction  vector,  direction  vector  can  point 
;  in  either  direction  between  endpoints 

;  ridge,  valley,  etc 


variables) 


(defmethod  (line-segment  :endpoints)  ()  ;get  endpoints  of  the  line  segment 
(send  (eval  direction-vector)  lendpoints)) 

(defmethod  (line-segment  :endpoint-list)  ()  ;  get  endpoints  in  graphics  format 
(list  (send  (eval  (car  (send  self  :endpoints)))  :list-format-4) 

(send  (eval  (cadr  (send  self  :endpoints)))  :list-format-4))) 


(defmethod  (line-segment  :other-end)  (endpoint)  :  find  the  endpoint  of  the  line-segment 
(let  ((llne-endpoints  (send  self  :endpoints))) ;  opposite  of  the  given  endpoint 
(cond  ((equal  endpoint  (first  line-endpoints)) 

(second  line-endpoints)) 

(t  (first  line-endpoints))))) 
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(defmethod  (line-segment  :stai1-point)  () ;  what  is  the  start  point  of  the  line-segment? 
(vector-start-point  (eval  direction-vector))) 

(defmethod  (line-segment  ;end-point)  () ;  what  is  tfie  end  point  of  the  line  segment? 
(vector-end-point  (eval  direction-vector))) 

(defnrethod  (line-segment  :length)  ()  ;  how  long  is  the  line-segment? 

(send  (eval  direction-vector)  :length)) 

(defmethod  (line-segment  rbacksubs)  (t-list)  ;  subs  a  list  of  t-parameters 

;  back  into  the  line  equation  to  get 
;  the  (x  y  z)  coord  of  the  point 

(mapcar  ’+  (send  (eval  position-vector)  :list-format-real) 

(mapcar  ’*  t-list 

(send  (eval  direction-vector)  :list-format-real)))) 

(defmethod  (line-segment  :midpoint)  () 

(let  ((t-half  (/  t-max  ’2.0))) 

(send  self  :backsubs  (list  t-half  t-haK  t-half)))) 

(defmethod  (line-segment  ;strattle-plane-p)  (plane) 

;  return  T  iff  the  endpoints  of  self 
;  are  on  opposite  sides  of  the  given  plane 
(let  ((Ao-1  (send  (eval  plane)  :point-into-equation 

(first  (send  self  ;endpoints)))) 

(Ao-2  (send  (eval  plane)  ;point-into-equation 

(second  (send  self  :endpoints)))) 

(Ao  (fourth  (send  (eval  plane)  :list-coeff)))) 

(cond  ((or  (equal-error  Ao  Ao-1) 

(equal-error  Ao  Ao-2)) 

’nil) 

((or  (and  (GE  Ao-1  Ao) 

(LE  Ao-2  Ao)) 

(and  (LE  Ao-1  Ao) 

(GE  Ao-2  Ao))) 

’t)))) 

(defmethod  (line-segment  :print)  () 

(pprint  t-max  *output-stream*) 

(pprint  (list  position-vector  (send  (eval  position-vector)  :list-format) 

(send  (eval  position-vector)  :endpoints))  ‘output-stream*) 

(pprint  (list  direction-vector  (send  (eval  direction-vector)  :list-fonmat) 

(send  (eval  direction-vector)  :endpoints))  ‘output-stream*) 

(pprint  (send  self  :endpoints)  ‘output-stream*) 

(pprint  characteristics  ‘output-stream*)) 


PLANE 
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(defflavor  plane 

;  uses  equation  of  plane; 

(a-coef 

• 

b-coef 

:  aX  +  bY  +  cZ  =  Ao 

c-coef 

Ao) 

;  for  comparisions,  equation  is  generaily 

0 

:  normalized,  so  Ao=-1,+1  or  0. 

:gettable-instance-variables  ;  NOTE:  first  non-zero  coeff  will  ALWAYS  be  a 
:settable-instance-variables  ;  positive  number.  Avoids  direction  ambiguity 
'.inittable-instance-variables 
loutside-accessible-instance-variables) 

(defmethod  (plane  lest-equal)  (F2)  ;  test  plane  for  equality  by  comparing 

;  coefficients,  or  comparing  the  coeffs 
;  of  the  unit  normal  vectors 

(let  ((VI  (inK-vector  ’‘origin*  (init-point  (send  seif  :list-coeff-3)))) 

(V2  (init-vector  ’‘origin*  (init-point  (send  (eval  F2)  :list-coeff-3))))) 

(or  (apply  ’and 

(map  ’list  #’ equal-error 

(send  self  :list-coeff) 

(send  (eval  F2)  :list-coeff))) 

(apply  ’and 

(map  ’list  #■  equal-error 

(send  (eval  VI)  :unit-vector) 

(send  (eval  V2)  :unit-vector)))))) 

(defmethod  (plane  :list-coeff)  ()  ;  list  plane  coefficents  as  a  4-tuple 
(list  a-coef  b-coef  c-coef  Ao))  ;  (includes  the  Ao  constant  term) 

(defmethod  (plane  :list-coeff-3)  ()  ;  list  only  the  x.y.z  coefficents 
(list  a-coef  b-coef  c-coef)) 

(defmethod  (plane  :subs-point-into-plane)  (R)  ;  subs  a  point  into  the  planar 

:  equation,  returns  result. 

(apply  '+  (map  ’list  ’*  (send  self  :list-coeff-3)  (send  (eval  Pt)  :list-format)))) 

(defmethod  (plane  ipoint-into-equation)  (point)  ;  subs  point  into  plane  equation 

;  same  as  above  “‘“REMOVE . 

(apply  ’+  (map  'list  ’*  (send  (eval  point)  :list-format) 

(send  seif :  list-coeff-3)))) 

(defmethod  (plane  iind-x-given-yz)  (y  z)  ;  find  the  x  value  of  a  point  given  the 
(cond  ((equal-zero-p  a-coef)  ’0)  ;  y  and  z  coordinates  of  a  point,  for 

;  the  plane  under  consideration 
(t  (/  (-  Ao  (+  (‘  b-coef  y)  (‘  c-coef  z)))  a-coef)))) 

(defmethod  (plane  :find-y-given-xz)  (x  z)  ;  find  the  y  value  of  a  point  given  the 
(cond  ((equal-zero-p  b-coef)  ’0)  ;  x  and  z  coordinates  of  a  point,  for 

;  the  plane  under  consideration 
(t  (/  (-  Ao  (+  (‘  a-coef  x)  (‘  c-coef  z)))  b-coef)))) 

(defmethod  (plane  :find-z-given-xy)  (x  y)  ;  find  the  z  value  of  a  point  given  the 
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(cond  {(equal-zero-p  c-coef)  ’0)  ;  x  and  y  coordinates  of  a  point,  for 

;  the  plane  under  consideration 
(t  (/  (-  Ao  (+  (*  a-coef  x)  (*  b-coef  y)))  c-coef)))) 

(defniethod  (plane  :print)  () 

(pprint  (send  self  ;list-coeff)  *output-stream*)) 


; - BOUNDING  BOX - 

(defflavor  Bounding-box  ;  generalized  bounding  box  flavor 

(max-x 
min-x 
max-y 
min-y 
max-z 
min-z) 

0 

:gettable-instance-variables 

isettable-instance-variables 

:inittable-instance-variables 

;outside-accessible-instance-variables 

:required-methods) 

(defmethod  (bounding-box  :construct-bounding-box)  (points) 

:  build  bounding  box  for 
;  a  list  of  points 

(let*  ((first-point  (send  (eval  (first  points))  :list-format)) 

(x  (first  first-point)) 

(y  (second  first-point)) 

(z  (third  first-point))) 

(self  max-x  x) 

(self  min-x  x) 

(self  max-y  y) 

(self  min-y  y) 

(self  max-z  z) 

(sett  min-z  z) 

(loop  for  P  in  (rest  points) 
do  (let*  ((next-point  (send  (eval  P)  :list-format)) 

(new-x  (first  next-point)) 

(new-y  (second  next-point)) 

(new-z  (third  next-point))) 

(cond  ((QT  new-x  max-x) 

(sett  max-x  new-x)) 

((LT  new-x  min-x) 

(sett  min-x  new-x))) 

(cond  ((GT  new-y  max-y) 

(sett  max-y  new-y)) 

((LT  new-y  min-y) 

(sett  min-y  new-y))) 
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(cond  ((GT  new-z  max-z) 

(self  max-z  new-z)) 

((LT  new-z  min-z) 

(sett  min-z  new-z))))))) 

(defmethod  (bounding-box  :inside-bounding-box-p)  (point) 

;  return  T  if  point  is  inside 
;  bounding  box,  NIL  otherwise 

(let  ((p  (map  'list  ’*  (send  (eval  point)  :llst-format)  ’(1.0  1.0  1.0)))) 
(cond  ((and  (and  (GE  max-x  (first  p)) 

(LE  min-x  (first  p))) 

(and  (GE  max-y  (second  p)) 

(LE  min-y  (second  p))) 

(and  (GE  max-z  (third  p)) 

(LE  min-z  (third  p)))) 

’t) 

(t  ’nil)))) 


: - FACET - 

(defflavor  facet 

(edges  ;list  of  all  edges  bounding  facet 

center  ;  location  of  center  of  facet 

connects)  ;  volumes  which  facet  connects  "((VI .. Vn)  (V2..Vm))" 

(plane  ;mixin  flavors 

bounding-box) 

'.gettable-instance-variables 

:settable-instance-variables 

:inittable-instance-variables 

:outside-accessible-instance-variables 

:required-methods) 

(defmethod  (facet  ;points)  () ;  return  all  verticies  of  facet 
(let  ((temp  ’nil)) 

(loop  for  E  in  Edges 

do  (setf  temp  (append  temp  (send  (eval  E)  :endpoints)))) 

(delete-duplicates  temp))) 

(defmethod  (facet  :find-facet-center)  ()  ;  find  the  average  of  all  the  vertices 

;  of  the  facet. 

(let*  ((points  (send  self  :points)) 

(temp-sum  (send  (eval  (first  points))  :list-format)) 

(nr-points  (length  points))) 

(loop  for  P  in  (rest  points) 
do  (setf  temp-sum  (map  ’list  ’+  temp-sum 

(send  (eval  P)  :list-format)))) 

(setf  (facet-center  self) 

(init-point  (map  ’list  ’/  temp-sum  (make-list  3  :initial-element  nr-points)))) 
(facet-center  self))) 
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(defmethod  (facet  :add-volume-to-left-connects)  (V) ;  add  a  volume  to  the  left  list 

;  of  the  connects  variable 
(cond  ((null  (facet-connects  self)) 

(setf  (facet-connects  self)  (list  (list  V)))) 

((not  (member-p  V  (first  (facet-connects  self)))) 

(setf  (first  (facet-connects  self))  (adjoin  V  (first  (facet-connects  self))))))) 

(defmethod  (facet  :add-volume-to-right-connects)  (V) ;  add  a  volume  to  the  right  list 

;  of  the  connects  variable 
(cond  ((equal  '1  (length  (facet-connects  self))) 

(setf  (facet-connects  seH)  (list  (first  (facet-connects  self))  (list  V)))) 

((not  (member-p  V  (second  (facet-connects  seH)))) 

(setf  (second  (facet-connects  self))  (adjoin  V  (second  (facet-connects  self))))))) 


(defmethod  (facet  ;print)  () 

(pprint  (list  edges  center  connects  (send  self  ;list-coeff))  'output-stream* )) 


; - VOLUME - 

(defflavor  volume 

(Visibility  ;  visible  observers 

Probability-of-detection  ;  sum  of  PD  for  observers 
Composition  ;  ground,  air,  etc 

Points  ;  all  vertices  of  the  volume 

Edges  ;  all  line-segments  of  the  volume 

Facets  ;  all  surfaces  of  the  volume 

Arithmetic-center  ;  numeric  average  of  the  points 
conr>ected-to)  ;  adjacent  volumes 

(Graphic)  ;  for  3-D  projection 

:gettable-instance- variables 
rsettable-instance- variables 
:inittable-instanoe-variables 
:outside-accessible-instance-variables 
.required-methods) 

(defmethod  (volume  :make-equal)  (new-volume-name) 

;  make  a  new  volume  with  same  instances 
(let  ((temp  new-volume-name))  ;  as  self 

(set  temp  (make-instance  Volume 
:Visibility  Visibility 

:Probability-of-detection  Probability-of-detection 

:Composition  Composition 

Points  Points 

:Edges  Edges 

Pacets  Facets 

:arlthmetic-center  Arithmetic-center 
:connected-to  Connected-to)))) 
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(defmethod  (volume  :dear)  {)  ;  clear  out  old  values  of  existing  volumes 
(setf  Visibility  ’nil) 

(setf  Probability-of-detection  ’nil) 

(setf  Composition  ’nil) 

(setf  Points  ’nil) 

(setf  Edges  ’nil) 

(setf  Facets  ’nil) 

(setf  Arithmetic-center  ’nil) 

(setf  Connected-to  ’nil)) 

(defmethod  (volume  :find-arithmetic-center)  ()  ;  find  the  average  of  all  the  vertices 

;  of  the  volume,  do  not  change  values 
;  in  the  volume 

(let  ((temp-sum  (send  (eval  (first  Points))  rlist-format)) 

(nr-points  (length  Points))) 

(loop  for  P  in  (rest  Points) 
do  (setf  temp-sum  (map  ’list  ’+  temp-sum 

(send  (evsd  P)  :list-format)))) 

(init-point  (map  ’list  ’/  temp-sum  (make-list  3  :lnitial-eiement  nr-points))))) 

(defmethod  (voiume  :make-node-iist)  () ;  make  a  list  of  absoiute  point  coords  in  graphic 
(ioop  for  P  in  points  ;  format  (eg  4  element  list) 

:  used  in  GRAPHICS. 

collect  (reverse  (append  (list  ’1)  (reverse  (send  (eval  P)  :list-format)))))) 

(defmethod  (volume  .make-polygon-list)  ()  ;lndex  point  values  to  points  in  node  list 
(loop  for  L  in  edges  ;  used  in  GRAPHICS 

do  (setf  Pti  (car  (send  (eval  L)  ;endpoint-list))) 
do  (setf  Pt2  (cadr  (send  (eval  L)  :endpoint-list))) 
collect  (list  (position-if  ’(lambda  (A)  (equal  A  R1))  node-list) 

(position-if  ’(lambda  (A)  (equal  A  R2))  node-list)))) 

(defmethod  (volume  :print)  () 

(pprint  (list  Visibility  Probability-of-detection  Composition  Points  Edges  Facets 
arithmetic-center  connected-to)  ‘output-stream*)) 


UNIVERSE 


(defflavor  Universe 
(Volumes 
Observers 
static-vectors 
static-vector-counter 
static-lines 
static-lines-counter 
static-points 
static-points-counter) 
0 

-gettable-instance-variables 


space  of  all  volumes 

:  observers  located  within  the  defined  universe 
;  save  the  state  of  the  lines,  points  and 
;  vectors  used  to  build  the  static  visibility 
model 
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:settable-instance-variables 

linittable-instance-variables 

:outside-accessible-instanoe-variables) 

(defmethod  (universe  :save-static-items)  ()  ;  save  state  of  static  universe 

(sett  static-vectors  *iist-of-vectors*) 

(sett  *nf)inimum-vectors-counter*  *vectors-counter*) 

(sett  static-iines  *iist-of-lines*) 

(sett  *minimum-iines-counter*  'iines-counter*) 

(sett  static-points  ‘list-of-points*) 

(sett  *minimum-points-counter*  *points-counter*) 

(sett  •minimum-planes-counter*  *planes-counter*) 

(sett  ‘minimum-faoets-counter*  *facets-counter*) 

(sett  *minimum-voiumes-counter*  *voiumes-counter*)) 


FUNCTiONS  TO  INITiALIZE;  GET  NAMES  OF  OBJECTS  AND  MAKE  NAMES  GLOBAL 


(defun  make-origin  ()  ;  names  of  special  points  and 

(gensym  (incf  *points-counter*))  ;  other  unique  flavors, 

(setf  ‘origin*  (make-instance  'point 

:x-coord  '0 
;y-coord  ’0 
:z-coord  ’0)) 

(pushnew  ’‘origin*  ‘list-of-points*)) 


(defun  make-nuli-vector  () 

(gensym  (incf  ‘vectors-counter*)) 

(setf  ‘null-vector*  (make-instance  'vector 

:i  '0 


:j'0 
:k  '0 

:Start-point  ’‘origin* 
:End-point  ’‘origin*)) 

(push  ’‘null-vector*  *list-of-vectors*)) 


(defun  make-point-name  ()  iproduce  variable  names  "on  the  fly" 

(cond  ((>  ‘points-counter*  (1-  ‘max-counter-value*)) 

(setf  ‘points-counter*  ‘minimum-points-counter*))) 

(gensym  (incf  ‘points-counter*)) 

(intern  (gensym  "point"))) 
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(defun  make-line-name  () 

(cond  ((>  •llnes-counter*  (1-  *max-counter-value*)) 

(setf  *lines-counter*  *minimum-lines-counter*))) 
(gensym  {incf  *lines-counter*)) 

(intern  (gensym  "line"))) 

(defun  make-vector-name  () 

(cond  ((>  "vectors-counter*  (1-  "max-counter- value*)) 

(setf  "vectors-counter*  "minimum-vectors-counter"))) 
(gensym  (Incf  "vectors-counter")) 

(intern  (gensym  "vector"))) 

(defun  make-facet-name  () 

(cond  ((>  "facets-counter*  (1-  "max-counter-value")) 

(setf  "facets-counter*  "minimum-facets-counter"))) 
(gensym  (Incf  "facets-counter")) 

(intern  (gensym  "facet"))) 

(defun  make-plane-name  () 

(cond  ((>  "planes-counter*  (1-  "max-counter-value")) 

(setf  "planes-counter*  "minimum-planes-counter"))) 
(gensym  (incf  "planes-counter")) 

(intern  (gensym  "plane"))) 

(defun  make-volume-name  () 

(cond  ((>  "volumes-counter*  (1-  "max-counter-value")) 

(setf  "volumes-counter*  "minimum-volumes-counter"))) 
(gensym  (incf  "volumes-counter")) 

(intern  (gensym  "volume"))) 


FLAVOR  INSTANTIATION  FUNCTIONS 

Note;  all  of  therse  functions  will  stop  keeping  lists  of  previously 
created  instantiations  after  flag 
"done-making-new-visibility-volumes-flag*  is  set  to  T 


: - MAKE  A  POINT - 

(defun  init-point  (List-of-values)  ;  see  if  point  already  exists  (nonrecursive) 
(cond  ((and  (not  (null  *list-of-points*)) 

(not  "done-making-new-visibility-volumes-flag")) 

(loop  for  P  in  *list-of-points* 
do  (cond  ((apply  'and 

(map  'list  #'equal-error 

(map  'list  ’rationalize  list-of-values) 

(send  (eval  P)  :list-format))) 
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(retum-from  Init-point  P)))))) 

(init-new-point  iist-of-values)) 

(defun  inH-new-point  (List-of-values) 

(let  ((temp  (make-point-name))) 

(set  temp  (make-instance  'point 

:x-cootd  (rationaiize  (first  List-of-values)) 

:y-coord  (rationalize  (second  List-of-values)) 

:z-coord  (rationalize  (third  Ust-of-values)))) 

(push  temp  Vlst-ol-polnts*) 
temp)) 

; - MAKE  A  VECTOR- - 

(defun  ink-vector  (Start-point  End-point)  ;  chedt  to  see  K  vector  already  built 
(cond  ((not  *done-making-new-visibility-volumes-flag*) 

(loop  for  V  in  *iist-of-vectors* 
do  (cond  ((equai  (send  (eval  V)  :endpoints) 

(iist  Start-point  End-point)) 

(retum-from  ink-vector  V)))))) 

(init-new-vector  Start-point  End-point)) 

(defun  ink-new-vector  (Sp  Ep) 

(let  ((temp  (make-vector-name))) 

(set  temp  (make-instance  'vector 

:i  (-  (point-x-coord  (eval  Ep))  (point-x-coord  (eval  Sp))) 
:J  (•  (point-y-coord  (eval  Ep))  (point-y-coord  (aval  Sp))) 
:k  (-  (point-z-coord  (eval  Ep))  (point-z-coord  (eval  Sp))) 
;Start-point  Sp 
:End-point  Ep)) 

(push  temp  *list-of-vectors*) 
temp)) 


; - MAKE  A  LINE  SEGMENT - 

(defun  ink-line  (Poskion- vector  Direction-vector)  ;  valid  construction  for  a  line??? 
(cond  ((and  (equal  (vector-Start-point  (eval  Position-vector))  '‘origin*) 
(member-p  (vector-end-point  (eval  Position-vector)) 

(send  (eval  Direction-vector)  rendpoints))) 
(Find-or-make-line  Position-vector  Direction-vector)) 

(t  (terpri) 

(princ  "Error  invalid  vectors: ') 

(print  (list  poskion-vector  direckon-vector))  (terpri)))) 

(defun  Find-or-make-line  (Pv  Dv)  ;  check  to  see  H  line  already  built 
(cond  ((not  *done-making-new-visibilky-volumes-flag*) 

(loop  for  L  in  *list-of-lines* 

do  (cond  ((and  (member-p  (vector-end-point  (eval  Pv)) 

(send  (eval  (old-line-Dv  L))  :endpoints)) 
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(or  (equal  (send  (eval  Dv)  :endpoints) 

(send  (eval  (old-line-Dv  L))  lendpoints)) 
(equal  (send  (eval  Ov)  .endpoints) 

(nreverse  (send  (eval  (old-line-Dv  L))  :endpoints))))) 
(retum-from  find-or-make-line  L)))))) 

(init-new-line  Pv  Dv)) 


(defun  init-new-line  (Pv  Dv) 

(let  ((temp  (make-line-name))) 

(set  temp  (make-instance  'line-segment 

:t-max  '1 


Position-vector  Pv 
:Direction-vector  Dv 
:characteristics  ’nil)) 

(push  temp  *list-of-lines*) 
temp)) 


(defun  old-line-Dv  (Line) 
(line-segment-Direction-vector  (eval  Line))) 


MAKE  A  PLANE 


(defun  ink-plane  (List-of-values)  ;  see  if  piane  already  exists  (nonrecursive) 
(cond  ((and  (not  (nuii  *list-of-pianes*)) 

(not  *done-making-new-visibiiity-volumes-flag*)) 

(loop  for  P  in  *list-of-planes* 
do  (cond  ((or  (equai  (send  (eval  P)  ;list-coeff) 

list-of-values) 

(apply  'and  (map  'list  #’equal-error 

(send  (eval  P)  :list-coeff) 
list-of-values))) 

(retum-from  init-plane  P)))))) 

(init-new-plane  list-of-values)) 


(defun  init-new-plane  (List-of-values) 

(let  ((temp  (make-plane-name))) 

(set  temp  (make-instance  'plane 

:a-coef  (rationalize  (first  list-of-values)) 
;b-coef  (rationalize  (second  list-of-values)) 
:c-coef  (rationalize  (third  list-of-values)) 
:Ao  (fourth  list-of-values))) 

(push  temp  *list-of-planes*) 
temp)) 


- make  all  facets - 

Used  by  intercept  routines  to  rebuild  volume  facets. 
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WARNING  *** 

Note:  Facets  MUST  be  convex  and  MUST  NOT  be  adjacent  to 
facets  in  the  same  volume  with  the  same  plane  equation 


;  Used  by  input  method  1  and  by  all  intercept  routines 

(defun  make-all-facets  (Volume) 

(reset-point-property-lists  Volume) 

;  initialize  point  'lines  property  list 
(loop  for  L  in  (Volume-edges  (eval  Volume)) 
do  (let*  ((endpoints  (send  (eval  L)  :endpoints)) 

(first-point  (first  endpoints)) 

(second-point  (second  endpoints))) 

(setf  (get  first-point  ’lines)  (adjoin  L  (get  first-point  ’lines))) 

(setf  (get  second-point  ’lines)  (adjoin  L  (get  second-point  ’lines))))) 

;  build  all  facets  from  points 

(loop  for  P  in  (volume-points  (eval  Volume)) ;  make  all  facets  possible 
do  (loop  for  L  in  (get  P  ’lines) 

do  (let*  ((other-end-L  (send  (eval  L)  :other-end  P))) 

(initialize-search  Volume  P  (list  L)  (List  other-end-L  P))))) 

(reset-point-property-lists  Volume)) 

(defun  initialize-search  (Volume  Goal  old-lines  old-points) 

(let  ((point2  (first  old-points)) 

(Line  (first  old-lines)) 

(search-result  ’nil) 

(facet-name  ’nil)) 

(loop  for  L  in  (get  point2  ’lines) 
do  (cond  ((and  (not  (equal  L  Line)) 

(not  (equal  Goal  (send  (eval  L)  ;other-end  point2)))) 

(let  ((plane  (init-plane  (make-a-normalized-plane  L  Line)))) 

(cond  ((not  (member-p  plane  (get  Goal  ’planes))) 

(setf  (get  Goal  ’planes)  (adjoin  plane  (get  Goal  ’planes))) 

(setf  search-result  (search-to-make-facet  Goal 
plane 
(list  L  Line) 

(pushnew  (send  (eval  L)  :other-end  point2) 
old-points) 

’nil 

’nil)) 

(cond  ((<-  ’3  (length  (first  search-result))) 

(setf  facet-name  (init-facet-2  search-result))) 

(t  (setf  facet-name  ’nil))) 

(cond  ((not  (null  facet-name)) 

(setf  (volume-facets  (eval  Volume)) 

(adjoin  facet-name  (volume-facets  (eval  Volume)))) 

)))))))))) 
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(defun  search-to-make-facet  (Goal 

Facet-plane 

old-llnes 

old-points 

rejected-points 

rejected-lines) 

(let  ((current-point  (first  old-points)) 

(last-line  (first  old-lines)) 

(Line  ’nil) 

(possible-paths  ’nil)) 

(loop  for  candidate-line  in  (get  current-point  ’lines) 
do  (let  ((other-end-cand-line 

(send  (eval  candidate-line)  :other-end  current-point))) 

(cond  ((apply  ’and  (list  (not  (member-p  csuidldate-line  old-lines)) 

(not  (member-p  candidate-line  rejected-iines)) 

(not  (member-p  otfier-end-cand-line 

rejected-points)))) 

(cond  ((not  (member-p  other-end-cand-line  oid-points)) 

(cond  ((send  (eval  facet-plane)  :test-equal 

(make-a-piane  other-end-cand-line 
(first  old-lines))) 

(setf  (get  other-end-cand-line  ’distance) 

(distance  Goal  other-end-cand-line)) 

(setf  possible-paths 

(adjoin  candidate-line  possible-paths))) 

(t  (pushnew  candidate-line  rejected-lines)))) 

((equal  other-end-cand-line  Goal) 

(loop  for  P  in  (adjoin  other-end-cand-line  old-points) 
do  (setf  (get  P  ’planes) 

(adjoin  Facet-plane  (get  P  ’planes)))) 
(retum-from  search-to-make-facet  (list 

(adjoin  candidate-line 
old-lines) 
facet-plane))) 

(t  (pushnew  candidate-line  rejected-lines)))) 

(t  (pushnew  candidate-line  rejected-lines))))) 

(cond  ((not  (null  possible-paths)) 

(setf  Line  (minimum-distance  possible-paths  current-point)) 

(push  Line  old-lines) 

(pushnew  (send  (eval  Line)  ;other-end  current-point)  old-points)) 

(t  (pushnew  last-line  rejected-lines)  ;  remove  last  line,  current  point 
(pushnew  current-point  rejected-points) ;  and  retrace  steps  (backtrack) 

(setf  old-lines  (rest  old-lines)) 

(setf  old-points  (rest  old-points)) 

(cond  ((>  2  (length  old-lines))  ;  baddracked  too  far? 

(retum-from  search-to-make-facet  ’nil))))) 

(search-to-make-facet  Goal  Facet-plane  old-lines  old-points 

rejected-points  rejected-lines))) 

(defun  init-facet-2  (properties)  ;  Check  to  see  if  already  built  facet 
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(cond  ((not  (null  properties))  ;  else  return  name  of  new  facet,  or  nil. 

(let*  ((edges  (first  properties)) 

(plane  (second  properties)) 

(test-plane  (map  ’list  'abs 

(map  ’list  ’*  (send  (eval  plane)  :list-coeff) 
*one-vector*))) 

(equal-flag ’t)) 

(cond  ((equal-p  test-plane  *zero-vector*) ;  remove  £vtifact  facets 
(retum-from  init-facet-2  ’nil))) 

(cond  ((not  (null  *llst-of-facets*)) 

(loop  for  F  in  *list-of-facets*  ;  see  if  already  exists 
do  (cond  ((equal  (length  edges) 

(length  (facet-edges  (eval  F)))) 

(setf  equal-flag ’t) 

(loop  for  E  in  edges 

do  (cond  ((not  (member-p  E  (facet-edges  (eval  F)))) 
(setf  equal-flag  ’nil)))) 

(cond  (equal-flag 

(retum-from  init-facet-2  F)))))))) 

(make-new-facet  edges  plane))) 

(t  (retum-from  init-faoet-2  ’nil)))) 

(defun  make-new-facet  (list-of-edges  plane) 

(let  ((plane-equation  (send  (eval  Plane)  :list-coeff)) 

(temp  (make-facet-name))) 

(set  temp  (make-instance  ’facet 

:  Edges  list-of-edges 
:center  ’nil 
connects  ’nil 

:a-coef  (first  Plane-equation) 

;b-coef  (second  Plane-equation) 

:c-coef  (third  Plane-equation) 

:Ao  (fourth  Plane-equation))) 

(push  temp  *list-of-facets*) 
temp)) 


; - make  a  facet  from  input- 

;  Used  by  input  method  2  (only) 


(defun  make-a-facet  (points)  ;  build  a  facet  from  a  list  of  point  names 
(let  ((first-point  (first  points)) 

(start-point  (first  points)) 

(lines  ’nil) 

(plane-of-facet  ’nil)) 

(loop  for  End-point  in  (rest  points)  ;  construct  edges  of  facet 
do  (let  0 

(setf  lines  (adjoin  (make-line  Start-point  End-point)  lines)) 

(setf  Start-point  End-point))) 

(setf  lines  (adjoin  (make-line  Start-point  First-point)  lines)) 
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(setf  Piane-of-facet  (inK-plane  (make-a-normallzed-plane  (first  lines) 

(second  lines)))) 

(make-new-facet  lines  plane-of-facet)))  ;  return  new  facet  name 


(defun  build-side-facet  (R1  Pt2  Side-points) ;  msdte  a  facet  w/disjoint  list  of  points 
(make-a-faoet  (append  (list  R1  Pt2)  Side-points))) 

(defun  build-terrain  (data)  ;  build  facets  with  raw  facet  data,  where  data 
;  Is  In  format  (point  point  point ...) 

;  and  the  points  are  in  format  (x  y  z) 

;  return  a  list  of  all  facets  built 
(let  ((list-of-facets  ’nil)) 

(loop  for  Facets  in  Data  ;  each  list  within  data  is  a  facet 
do  (let  ((points  (map  'list  finit-point  Facets))) 

(setf  list-of-facets  (adjoin  (make-a-facet  points)  list-of-facets)))) 
list-of-facets)) 


MAKE  A  VOLUME 


(defun  init-volume  (data) 

(let  ((temp  (make-volume-name)) 

(volume-data  (pop  data))) 

(set  temp  (make-instance  'volume 

;  Visibility  (second  volume-data) 
;Probability-of-detection  'nil 
•.Composition  (first  volume-data) 
rPoints  ’() 

:Edges  ’() 

:Facets  ’() 

:arithmetic-center  'nil 
:connected-to  ’nil)) 

(push  temp  *list-of-volumes*) 

(Initialize-volume  temp  data) 

temp))  ;  return  name  of  volume  created 
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FILE  NAME:  Visibility-functions • lisp 


Mode;Common-Lisp;  Base:  10 


VISIBILITY  AND  RIDGES 

This  file  contains  both  the  visibility  detennination  code 
and  the  ridge  creation  and  initial  air-volume  'convexizing' 
code.  The  visibility  code  is  first,  foliowed  by  the  ridge 
code. 

THESIS  D.H.  Lewis  10/1 1/86 


ViSIBILITY  REQiONS  D.H.  Lewis  10  Aug  88 


Contains  the  Observer  flavor;  code  for  aeating  and 
manipulating  observer  data;  code  for  making  visibility 
visibility  regions;  code  for  determining  the  visibiiity  of 
visibility  volumes;  and  finally  code  for  finding  the  probability 
of  detection  for  the  visibility  volumes. 

Main  functions:  MAKE-VISIBiLITY-REGiONS  (OBSERVER) 
DETERMINE-VISiBiLITY  (OBSERVER) 
INIT-OBSERVER  (COORDINATES  EFFECTIVNESS) 
CONNECT-VOLUMES  () 

Other  functions;  MAKE-OBSERVER-NAME 
COLINEAR-P 
FIND-T 

PROBABILITIES-ASSUMING-INDEPENDENCE-OR 

PROBABILITIES-ASSUMING-INDEPENDENCE-AND 

CLEAR-VISIBILfTY 

MATCH-FACET-WITH-ANOTHER-VOLUME 

SHOW-CONNECTIVITY 

CLEAR-CONNECTIVITY 

CONNECTIVITY-METRIC 


(defvar  *list-of-observers*  'nil) 
(defvar  ‘observer-counter*  ’0) 


FLAVORS  USED  TO  CREATE  OR  MANIPULATE  VISIBILITY  REQIONS 


(defflavor  Observer 
(Effectivness 
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Position) 

(graphic)  ;  for  dispiay 
;gettabie-instance-variabies 
:settable-instance-variabies 
linittabie-instance-variabies 
:outsicie-accessible-instance-variables) 

; - METHODS  FOR  OBSERVERS - 

(defmethod  (observer  :make-node-list)  () 

(iist  (reverse  (append  (iist  ’1)  (reverse  (send  (eval  position)  :iist-format)))))) 

(defmethod  (observer  :make-poiygon-iist)  () 

’((0  0))) 


; - FUNCTiONS  FOR  OBSERVERS 

(defun  make-observer-name  () 

(gensym  (incf  ‘observer-counter*)) 

(intern  (gensym  "observer"))) 


(defun  init-observer  (coord  effectivness) 

(let*  ((temp  (make-observer-name)) 

(position  (init-point  coord)) 

(volume-location  (locate-point-air  position)))  ;which  air  volumes  contain  obs? 
(cond  ((null  volume-location)  ;mak6  sure  not  underground 

(terpri) 

(princ  "Invalid  location  for  observer  (underground)")  (terpri) 

(return-from  init-observer  ’nil))) 

(set  temp  (make-instance  ’Observer 

;  Effectivness  effectivness 
Position  position)) 

(pushnew  temp  *list-of-observers*) 

(setf  (universe-observers  'universe*)  (adjoin  temp 

(universe-observers  'universe*))) 

temp)) 


Determine  all  observer  planes,  and  make  visibility  regions 


(defun  make-visibility-regions  (observer) 

(let  ((ground-volumes  ’nil) 

(air-volumes  ’nil) 

(ridges  ’nil) 

(planes  ’nil) 

(result- volume-list  ’nil)) 

;  find  all  air, ground  volumes,  visible  ridges 

(terpri)  (terpri) 
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(princ  "making  visibility  regions  for: ") 

(print  observer)  (terpri)  (terpri) 

(loop  for  V  in  (universe-volumes  "universe") 
do  (cond  ((equal  ’ground  (volume-composHion  (eval  V))) 

(setf  ground-volumes  (adjoin  V  ground-volumes)) 

(loop  for  L  in  (volume-edges  (eval  V)) 

do  (cond  ((equal  'ridge  (line-segment-characteristics  (eval  L))) 

(cond  ((not  (colinear-p  (observer-position  (eval  observer)) 

L)) 

(setf  ridges  (adjoin  L  ridges)))))))) 

(t  (setf  air-volumes  (adjoin  (list  V)  air-volumes)) 

(setf  (universe-volumes  "universe") 

(remove  V  (universe-volumes  "universe")))))) 

;  make  all  visibility  limiting  planes 

(loop  for  R  in  ridges 

do  (setf  planes  (adjoin  (make-a-plane  (observer-position  (eval  Observer))  R) 

planes))) 

;  intersect  all  air  volumes  with  planes 
(princ  "Air  volumes: ")  (print  air-volumes)  (terpri) 

(princ  "Limiting  planes  of  visibility: ")  (print  planes)  (terpri)  (terpri) 

(setf  result-volume-list  (intersect-all-planes-with-volumes  planes 

air-volumes)) 

(loop  for  V  in  result-volume-list 
do  (push  (car  V)  (universe-volumes  "universe")))) 

(send  "universe"  :save-static-items)  ;  save  the  state  of  the  static  model 
(universe-volumes  "universe")) 

(defun  colinear-p  (point  line) 

(let  ((tx  (find-t  '0  point  line))  ;  find  x,y,z  t  parameters 
(ty  (find-t ’t  point  line)) 

(tz  (find-t  ’2  point  line)) 

(t-list  ’nil) 

(t-list-reduced  ’nil)) 

(setf  t-list  (substitute  ’0.0  ’nil  (list  tx  ty  tz))) 

(setf  t-list-reduced  (remove  ’nil  (list  tx  ty  tz))) 

(cond  ((equal  ’1  (length  t-list-reduced)) 

(retum-from  colinear-p 

(apply  ’and  (mapcar  ’equal-error  (send  (eval  point)  :Iist-format-real) 

(send  (eval  line)  :backsubs  t-list))))) 

((equal  ’2  (length  t-list-reduced)) 

(retum-from  colinear-p  (apply  ’equal-error  t-list-reduced))) 

(t  (return-from  colinear-p  (and  (equal-error  tx  ty) 

(equal-error  tx  tz))))))) 


(defun  find-t  (nr  point  line) 

(let  ((denom  (nth  nr  (send  (eval  (line-segment-direction-veclor 

(eval  line)))  :list-format))) 

(numerator  (-  (nth  nr  (send  (eval  point)  :list-format)) 

(nth  nr  (send  (eval  (line-segment-position-vector 
(eval  line)))  :list-format))))) 

(cond  ((equal-zero-p  denom) 
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(retum-from  find-t  ’nil)) 

(t  (return-from  find-t  (/  numerator  denom)))))) 


Determine  visibility  of  visibility  regions 


(defun  determine-visibility  (observer) 

(determine- visibility- 1  observer)) 

(defun  determine-visibility- 1  (observer) 

;  determine  the  visibility  status  (yes  or  no) 

;  of  all  air  volumes  w/  respect  to  a  sigle  observer 
;  using  a  fast  method 

(terpri)  (terpri) 

(princ  "Visibility  determination  for:  *)  (print  observer) 

(terpri)  (terpri) 

(let  ((ground-volumes  ’nil) 

(air-volumes  ’nil) 

(ground-facets  ’nil) 

(volumes-containing-observer 

(locate-point-air  (observer-position  (eval  observer))))) 

;  find  all  air, ground  volumes,  and  ground  facets 
;  make  bounding  boxes  for  ground  facets 

(set-arithmetic-centers) 

(loop  for  V  in  volumes-containing-observer 
do  (princ "  ") 
do  (print  V) 
do  (princ "  visible") 
do  (terpri)) 

(loop  for  V  in  (universe-volumes  "universe") 
do  (cond  ((equal  ’air  (volume-composition  (eval  V))) 

(cond  ((not  (member-p  V  volumes-containing-observer)) 
(setf  air-volumes  (adjoin  V  air-volumes))))) 

(t  (setf  ground-volumes  (adjoin  V  ground-volumes)) 

(loop  for  F  in  (volume-facets  (eval  V)) 
do  (setf  ground-facets  (adjoin  F  ground-facets)))))) 

;  build  bounding  box  for  ground  facets 

(loop  for  F  in  ground-facets 

do  (send  (eval  F)  :construct-bounding-box  (send  (eval  F)  :points))) 

;  determine  visibility  of  all  air  volumes 
;  containg  the  observer 

(loop  for  V  in  volumes-containing-observer 
do  (setf  (volume-visibility  (eval  V)) 
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(adjoin  observer  (volume-visibility  (evai  V))))) 


;  determine  visibiiity  of  remainder  of  air  volumes 
;  by  seeing  if  visibility  line  intersects  a  ground 
;  facet 

(loop  for  V  in  air-volumes 

do  (let  ((visibility-line  (make-line  (observer-position  (evai  observer)) 

(volume-arithmetic-center  (evai  V)))) 

(biocked-fiag  ’nii)) 

(loop  for  F  In  ground-facets 

do  (let  ((facet-plane  (init-plane  (send  (evai  F)  :list-coeff))) 

(I  ’nil)) 

(cond  ((subs-line-into-plane-equation  visibility-line  facet-plane)) 
((not  blocked-flag) 

(cond  ((send  (evai  visibility-line)  ;strattle-plane-p 

facet-plane) 

(sett  I  (find-intercept-point  facet-plane 

visibility-line)) 

(cond  ((send  (evai  F)  ;inside-bounding-box-p  I) 
(cond  ((inside-facet-p  I  F) 

(princ "  ")  (print  V) 

(princ  ■  not  visible")  (terpri) 

(sett  blocked-flag ’t))))))))))) 

(cond  ((not  blocked-flag) 

(princ  "  ")  (print  V)  (princ "  visible")  (terpri) 

(setf  (volume-visibility  (evai  V)) 

(adjoin  observer  (volume-visibility  (evai  V)))))))) 

(terpri) 

’nil)) 


(defun  determine-visibility-2  (observer) 

;  determine  the  visibility  status  (yes  or  no) 

;  of  all  air  volumes  w/  respect  to  a  sigle  observer 
:  using  a  slow  method 

(terpri)  (terpri) 

(princ  "Visibility  determination  for: ")  (print  observer) 

(terpri)  (terpri) 

(let  ((ground-volumes  ’nil) 

(air-volumes  ’nil) 

(ground-facets  ’nil) 

(volumes-containing-observer 
(locate-point-air  (observer-position  (evai  observer))))) 
(set-arithmetic-centers) 

;  determine  visibility  of  all  air  volumes 
;  containg  the  observer 

(loop  for  V  in  volumes-containing-observer 
do  (setf  (volume-visibility  (evai  V)) 


161 


(adjoin  observer  (volume-visibility  (eval  V))))) 
(loop  for  V  in  volumes-containing-observer 
do(princ"  ") 
do  (print  V) 
do  (princ '  visible”) 
do  (terpri)) 


;  find  who  rest  of  volumes  are,  and  make  list 
;  of  blocking  ground  facets.  Remove  all 
;  vertical  ground  facets. 

(loop  for  V  in  (universe-volumes  ’universe”) 
do  (cond  ((equal  'air  (volume-composition  (eval  V))) 

(cond  ((not  (member-p  V  volumes-containing-observer)) 

(setf  air-volumes  (adjoin  V  air-volumes))))) 

(t  (setf  ground-volumes  (adjoin  V  ground- volumes)) 

(loop  for  F  in  (volume-facets  (eval  V)) 
do  (cond  ((and  (member-p  '0  (send  (eval  F)  ;list-coeff-3)) 

(>  2  (length  (remove  ’0  (send  (eval  F) 

:list-coeff-3)))))) 

(t  (setf  ground-facets  (adjoin  F  ground-facets)))))))) 

(setf  ground-facets  (remove-duplicates  ground-facets)) 

(loop  for  F  in  ground-facets 

do  (send  (eval  F)  :construct-bounding-box  (send  (eval  F)  :points))) 

;  determine  visibility  of  remainder  of  air  volumes 
;  by  seeing  if  visibility  line  intersects  a  ground 
:  facet 

(loop  for  V  in  air-volumes 

do  (let  ((visibility-line  (make-line  (observer-position  (eval  observer)) 

(volume-arithmetic-center  (eval  V))))) 

(cond  ((find-if-visibility-line-blocked-p  visibility-line 

ground-facets 

ground-volumes) 

(princ  "  ')  (print  V) 

(princ "  not  visible")  (terpri)) 

(t  (princ  "  ")  (print  V)  (princ "  visible”)  (terpri) 

(setf  (volume-visibility  (eval  V)) 

(adjoin  observer  (volume-visibility  (eval 

V)))))))) 

’nil)) 

(defun  find-if-visibility-line-blocked-p  (visibility-line 

ground-facets 

ground-volumes) 

(loop  for  F  in  ground-facets 
do  (let  ((intercept-point  (find-intercept-point 

(init-plane  (send  (eval  F)  :list-coeff)) 
visibility-line)) 

(location-volumes  ’nil)) 
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(cond  ((null  intercept-point) 

(retum-from  find-if-visibility-Rne-blocked-p  ’nil)) 

((not  (send  (eval  F)  :lnside-bounding-box-p  intercept-point)) 
(retum-from  find-if-visibility-line-blocked-p ’t)) 

(t  (sett  iocation-volumes  (locate-point  intercept-point)) 

(ioop  for  V  in  ground-voiumes 
do  (cond  ((noember-p  V  location-volumes) 

(retum-from  find-H-visiblllty-line-blocked-p ’t)))) 
(retum-from  find-if-visibility-line-biocked-p  ’nil)))))) 


(defun  probabilities-assuming-independence-or  (volume) 

;  set  volume  probability  of  detection  using  an 
;  asssumption  of  indepedence  between  observers,  and 
;  an  "or"  combination  technique 

(let  ((temp  '1 .0)) 

(terpri) 

(print  volume)  (princ  "  has  P.D.: ") 

(cond  ((not  (null  (volume-visibility  (eval  volume)))) 

(loop  for  Obs  in  (volume-visibility  (eval  volume)) 
do  (setf  temp  (*  temp  (-  '1.0  (observer-effectivness  (eval  Obs)))))) 

(setf  (volume-probability-of-detection  (eval  volume))  (-  '1.0  temp)) 

(print  (- ’1.0  temp))) 

(t  (setf  (volume-probability-of-detection  (eval  volume))  ’0.0) 

(print  -0.0))))) 

(defun  probabilities-assuming-independence-and  (volume) 

;  set  volume  probability  of  detection  using  an 
;  asssumption  of  indepedence  between  observers,  and 
:  an  "and"  combination  technique 

(let  ((temp  ’1.0)) 

(teipri) 

(print  volume)  (princ  "  has  P.D.; ") 

(cond  ((not  (null  (volume-visibility  (eval  volume)))) 

(loop  for  Obs  in  (volume-visibility  (eval  volume)) 
do  (setf  temp  (*  temp  (observer-effectivness  (eval  Obs))))) 

(setf  (volume-probability-of-detection  (eval  volume))  temp) 

(print  temp)) 

(t  (setf  (volume-probability-of-detection  (eval  volume))  ’0.0) 

(print  ’0.0))))) 

(defun  set-arithmetic-centers  () 

(loop  for  V  in  (universe-volumes  "universe") 

do  (setf  (volume-arithmetio-center  (eval  V))  (send  (eval  V)  :find-arithmetic-center)))) 


(defun  dear-visibility  ()  ;  clear  out  observer  visibility  info 

(loop  for  V  in  (uni verse- volumes  "universe") 
do  (setf  (volume-probability-of-detection  (eval  V))  ’nil) 
do  (setf  (volume-visibility  (eval  V))  ’nil)) 

’Done) 
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(defun  set-zero-PD  ()  ;  set  all  air  volume  PD's  to  zero 

(loop  for  V  in  (universe-volumes  ‘universe*) 
do  (cond  ((equal  ’air  (volume-composition  (eval  V))) 

(sett  (volume-probability-of-detection  (eval  V))  ’0.0)))) 

’done) 


CONNECTIVITY 


Connectivity  between  volumes 


(defun  Connect- volumes  ()  ;  connect  all  air  volumes  by  facets. 

(let  ((volumes  (universe-volumes  ‘universe*))) 

(terpri) 

(terpri)  (princ  "Connecting  volumes:")  (terpri)  (terpri) 

(loop  for  V  in  volumes 
do  (print  V) 

do  (princ "  Connected  to: ") 

do  (sett  (volume-connected-to  (eval  V))  ’nil) 

do  (cond  ((equal  ’air  (volume-composition  (eval  V))) 

(loop  for  F  in  (volume-facets  (eval  V)) 
do  (send  (eval  F)  :flnd-facet-center) 
do  (send  (eval  F)  :add-volume-to-left-connects  V) 
do  (let  ((match  (match-facet-with-another-volume  F  V))) 

(cond  ((and 

(not  (null  match)) 

(not  (equal  'ground  (volume-composition  (eval  match))))) 

(send  (eval  F)  :add-volume-to-right-connects  match)) 

((null  match) 

(let*  ((volumes  (locate-point-air  (facet-center  (eval  F))))) 

(loop  for  Connect-vol  in  (remove  V  volumes) 

do  (send  (eval  F)  :add-volume-to-right-connects  Con¬ 
nect-vol) 


)))))))) 

(loop  for  F  in  (volume-facets  (eval  V)) 

do  (sett  (volume-connected-to  (eval  V)) 

(append  (second  (facet-connects  (eval  F))) 
(volume-connected-to  (eval  V))))) 
(sett  (volume-connected-to  (eval  V)) 

(remove-duplicates  (volume-connected-to  (eval  V)))) 
(sett  (volume-connected-to  (eval  V)) 

(remove  ’nil  (volume-connected-to  (eval  V)))) 

(setf  (volume-connected-to  (eval  V)) 
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(remove  V  (volume-connected-to  (eval  V)))) 

(loop  for  V2  in  (volume-connected-to  (eval  V))  ;  remove  ground  volumes 

do  (cond  ((equal  'ground  (volume-composition  (eval  V2))) 

(setf  (volume-connected-to  (eval  V)) 

(remove  V2  (volume-connected-to  (eval  V))))))) 

(print  (volume-connected-to  (eval  V))) 

(terpri)) 

(terpri))) 

(defun  match-facet-wlth-another-volume  (Facet  Volume) 

;  return  the  name  of  the  unique  facet  which  is  shared 
;  between  two  volumes,  else  return  NIL.  Volume  is 
;  assumed  to  contain  facet 
(let  ((volumes  (universe-volumes  'universe*))) 

(loop  for  V  in  volumes 
do  (cond  ((not  (equal  V  Volume)) 

(cond  ((member-p  Facet  (volume-facets  (eval  V))) 

(retum-from  match-facet-with-another-volume  V)) 

((or  (member-p  V  (second  (facet-connects  (eval  Facet)))) 
(member-p  V  (first  (facet-connects  (eval  Facet))))) 
(return-from  match-facet-with-another-volume  V)))))) 
’nil)) 

(defun  show-connectivity  ()  ;  show  how  volumes  connect 

(terpri) 

(loop  for  V  in  (universe-volumes  'universe') 
do  (let  0 

(terpri)  (print  V) 

(princ "  <-> ") 

(print  (volume-connected-to  (eval  V)))))) 


(defun  dear-connectivity  ()  ;  clear  state  of  connectivity 

(loop  for  V  in  (universe-volumes  'universe') 
do  (setf  (volume-connected-to  (eval  V))  ’nil)) 

’done) 

(defun  connectivity-metric  () 

(terpri) 

(loop  for  V  in  (universe-volumes  'universe') 
do  (print  V) 

do  (princ Connections: ") 

do  (print  (length  (volume-connected-to  (eval  V)))) 

do  (princ  *  Facets: ") 

do  (print  (length  (volume-facets  (eval  V)))) 

do  (cond  ((or  (equal  (length  (volume-connected-to  (eval  V))) 

(1-  (length  (volume-facets  (eval  V))))) 

(equal  (length  (volume-connected-to  (eval  V))) 
(length  (volume-facets  (eval  V)))))) 

(t  (princ  "  “  possible  error"))) 
do  (terpri))) 
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RIDGE  CREATION  AND  MANIPULATION  FUNCTIONS 
D.H.  LEWIS  22May88 


Functions  to  find,  make,  and  manipulate  ridge  lines. 

Main  functions:  FIND-ALL-RIDGES  {) 

LINE-IS-A-RIDGE-P  (LINE  VOLUME) 
MAKE-CONVEX- VOLUMES  () 

Other  functions;  FIND-FACETS-WHICH-CONTAIN-EDGE 
PUT-FACET-ON-CORRECT-SIDE 
FIND-OVERLAPPING-FACETS 
FIND-HIGHEST-FACET 
RIDGE-LENGTH-SORT 


; — Make  ridges — 

(defun  find-all-ridges  ()  ;  look  for  line-segments  which  are  ridges 
(terpri)  (terpri) 

(princ  "Find  all  ridges  in  ground  terrain: ')  (terpri)  (terpri) 

(loop  for  Volume  in  (universe-volumes  "universe") 
do  (cond  ((equal  ’ground  (volume-composition  (eval  Volume))) 
(loop  for  E  in  (Volume-edges  (eval  Volume)) 
do  (princ  "Ridge  check,  line: ") 
do  (prini  E) 

do  (cond  ((line-is-a-ridge-p  E  Volume) 

(setf  (line-segment-characteristics  (eval  E)) 
’ridge) 

(princ " "  Ridge") 

(terpri)) 

(t  (setf  (line-segment-characteristics  (eval  E)) 
’nil) 

(terpri)))))))) 

(defun  line-ls-a-ridge-p  (Line  Volume)  ;  T  If  line  is  a  ridge 
(let  ((Facets  (find-facets-which-contain-edge  Line  Volume)) 
(Edge-vertical-plane  (make-vertical-piane  Line)) 
(Right-side-facets  ’nil) 

(Highest-right-side-facet  ’nil) 

(L<7ii-Sid6-fai./oi9  'iili/ 

(Highest-left-side-facet  ’nil) 

(Vertical-facets  ’nil) 

(Overtapping-facets  ’nil)) 


;  divide  facets  into  ieft  and  right  haives  based 
;  on  special  relationship  of  middle  point 
;  with  vertical  plane  of  Line 

(loop  for  F  in  facets 

do  (setf  (get  F  ’center)  (init-point  (mean-point-in-facet  F ))) 
do  (setf  (get  F  ’opposite-points)  ’nil) 

do  (let  ((side  (put-facet-on-correct-side  F  Edge-veiUcal-plane))) 

(cond  ((not  (null  (first  side))) 

(setf  Left-side-facets  (adjoin  (first  side)  Left-side-facets))) 

((not  (null  (second  side))) 

(setf  Vertical-facets  (adjoin  (second  side)  Verticai-facets))) 

((not  (nuil  (third  side))) 

(sett  Right-side-facets  (adjoin  (third  side)  Right-side-facets)))))) 

;  do  not  consider  vertical  facets  in  any  manner 

(cond  ((not  (null  Vertical-facets)) 

(return-from  Line-is-a-ridge-p  ’nil))) 

;  handle  overlapping  facets  by  creating  a  new  facet  center 
;  composed  of  average  of  facet  points  on  correct  side  of 
;  possibie  ridge  line 

(cond  ((or  (nuil  Left-side-facets) 

(null  Right-side-facets)) 

(cond  ((null  Left-side-facets) 

(setf  Overlapping-facets  (find-overlapping-facets  Edge-verticai-plane 

Right-side-facets)) 

(ioop  for  F  in  Overlapping-facets 
do  (setf  Right-side-facets  (remove  F  Right-side-facets)))) 

((null  Right-side-facets) 

(setf  Overlapping-facets  (find-overiapping-facets  Edge-verticai-plane 

Left-side-facets)) 

(loop  for  F  in  Overlapping-facets 
do  (setf  Left-side-facets  (remove  F  Left-side-facets))))) 

(cond  ((null  Overlapping-facets)  ;  have  an  internal  facet 

(retum-from  line-is-a-ridge-p  ’nil))) 

(loop  for  F  in  Overlapping-facets 
do  (setf  (get  F  ’center)  (init-point  (average-of-points 

(get  F  ’opposite-points)))) 

do  (let  ((side  (put-facet-on-correct-slde  F  Edge-verticai-plane))) 

(cond  ((not  (null  (first  side))) 

(setf  Left-side-facets  (adjoin  (first  side)  Left-side-facets))) 

((not  (null  (second  side))) 

(setf  Vertical-facets  (adjoin  (second  side)  Vertical-facets))) 

((not  (null  (third  side))) 

(setf  Right-side-facets  (adjoin  (third  side) 

Right-side-facets)))))))) 
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:  reduce  lists  of  left-  and  right-  facets  to  one  facet 
;  per  side,  based  upon  z-value  of  mean  point  of  facet 

(cond  ((<  1  (length  Left-side-facets)) 

(setf  Highest-left-side-facet  (find-highest-facet  Left-side-facets))) 

(t  (setf  Highest-left-side-facet  (first  Left-side-facets)))) 

(cond  ((<  1  (length  Right-side-facets)) 

(setf  Highest-right-side-facet  (find-highest-facet  Right-side-facets))) 
(t  (setf  Highest-right-side-facet  (first  Right-side-facets)))) 

;  find  if  line  is  a  ridge  by  subs  right  side  mean  value 
;  into  left-side  plane  equation,  if  resultant  Z  value 
;  is  greater  than  right-side  mean  value  z-value,  have 
;  a  ridge,  else  not 

(let*  ((point  (send  (eval  (get  Highest-right-side-facet  ’center))  :list-format)) 
(z-right-point-into-left-plane 
(send  (eval  Highest-left-side-facet) 

:find-z-given-xy  (first  point)  (second  point)))) 

(cond  ((>  z-right-point-into-left-plane  (third  point)) 

(retum-from  line-is-a-ridge-p ’t)) 

(t  (return-from  line-is-a-ridge-p  ’nil)))))) 


(defun  find-facets-which-contain-edge  (Edge  Volume) 

(let  ((temp  ’nil)) 

(loop  for  F  in  (volume-facets  (eval  Volume)) 
do  (cond  ((member-p  Edge  (facet-edges  (eval  F))) 

(setf  temp  (adjoin  F  temp))))) 

temp)) 

(defun  put-facet-on-correct-side  (Facet  Plane  ) 

(let*  ((Ao  (fourth  (send  (eval  plane)  :list-coeff))) 

(Ao-Point  (subs-point-into-equation  (send  (eval  plane)  :list-coeff-3) 

(get  Facet  ’center))) 

(Left  ’nil) 

(Vertical  ’nil) 

(Right  ’nil)) 

(cond  ((GT  Ao  Ao-point) 

(pushnew  Facet  Left)) 

((LT  Ao  Ao-point) 

(pushnew  Facet  Right)) 

(f  (pushnew  Facet  Vertical))) 

(list  (first  Left)  (first  Vertical)  (first  Right)))) 


(defun  find-overlapping-facets  (Vertical-plane  Facets) 

(let*  ((Line-Ao  (fourth  (send  (eval  vertical-plane)  :list-coeff))) 
(Facet-center-Ao  ’nil) 

(overlapping-facets  ’nil)) 
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(loop  for  F  in  Facets 

do  (sett  facet-center-Ao  (send  (eval  Vertical-plane)  :subs-point-into-plane 

(get  F  ’center))) 

do  (loop  for  P  in  (send  (eval  F)  :points) 

do  (let  ((Point-Ao  (send  (eval  Vertical-plane)  :subs-point-into-plane  P))) 
(cond  ((or  (and  (GT  Line-Ao  PoInt-Ao) 

(LT  Une-Ao  Facet-center-Ao)) 

(and  (LT  Line-Ao  PoInt-Ao) 

(GT  Line-Ao  Facet-center-Ao))) 

(setf  overlapping-facets  (adjoin  F  overlapping-facets)) 

(setf  (get  F  ’opposite-points) 

(adjoin  P  (get  F  ’opposite-points)))))))) 

overlapping-facets)) 


(defun  find-highest-facet  (List-of-facets) 

(let  ((highest-z  (third 

(send  (eval  (get  (first  list-of-facets)  ’center))  :list-format))) 
(highest-facet  (first  List-of-facets))) 

(loop  for  F  in  (rest  List-of-facets) 

do  (let  ((z  (third  (send  (eval  (get  F  ’center))  Jist-format)))) 

(cond  ((GT  z  highest-z) 

(setf  highest-z  z) 

(setf  highest-facet  F))))) 

highest-facet)) 


Use  ridges  to  make  convex  air  volumes — 


(defun  make-convex-volumes  ()  ;  intersect  all  vertical  planes  from  ridge 

(let  ((air-volume-list '())  ;  line-segments  with  all  volume(s). 

(volume-list  ’nil)  ;  Makes  all  air  volumes  convex, 

(ridge-list  ’nil)  ;  guarenteed. 

(plane-list  ’nil)) 

(terpri)  (terpri) 

(princ  "Making  air  volumes  convex:") 

(terpri)  (terpri) 


;  seperate  all  air  and  ground  volumes 
;  and  find  ridge  lines 

(loop  for  V  in  (Universe-volumes  "universe") 

do  (cond  ((equal  ’air  (volume-composition  (eval  V))) 

(setf  air-volume-list  (adjoin  (list  V)  air-volume-list)) 
(loop  for  E  in  (volume-edges  (eval  V)) 
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do  (<X)nd  ((equal  'ridge  (llne-segment-characteristics  (eval  E))) 
(setf  ridge-list  (adjoin  E  ridge-list))))) 

(sett  (universe-volumes  "universe*) 

(remove  V  (universe-volumes  ‘universe*)))))) 

;  reduce  list  of  ridge  lines,  and  construct  vertical  planes 

;  for  them,  ridges  are  sorted  by  length,  longest  first 

(setf  ridge-list  (remove-duplicates  ridge-list)) 

(setf  ridge-list  (remove  'nil  ridge-list)) 

(setf  ridge-list  (stable-sort  ridge-list  fridge-length-sort)) 

(loop  for  R  in  ridge-list 

do  (setf  plane-list  (adjoin  (make-verticai-plane  R)  plane-list))) 

(setf  plane-list  (reverse  plane-list)) 

(princ  "Air  volumes: ")  (print  air-volume-list)  (terpri) 

(princ  "Ridge  planes: ")  (print  plane-list)  (terpri)  (terpri) 

;  intersect  all  ridge  planes  with  all  air  volumes 
(setf  volume-list  (intersect-all-planes-with-votumes  plane-list 

air-volume-list)) 

;  update  universe  with  new  volumes  created 
(loop  for  V  in  volume-list 

do  (push  (car  V)  (universe-volumes  "universe*))) 

(universe-volumes  ‘universe*))) 


(defun  ridge-length-sort  (A  B)  ;return  T  iff  A  >  B 

(>  (send  (eval  A)  :length) 

(send  (eval  B)  :length))) 
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FILE  NAME:  Path-planning . lisp 
-*-  Mode:Common-Lisp;  Base:10 

:  PATH  PLANNING  D.H.  Lewis  25  Aug  88 


Contains  the  flavors,  methods,  and  functions  nessesary  to  conduct  path 
planning.  Divided  into  four  main  sections;  Flavors,  A-star 
path  planning,  and  path  optimization. 

The  flavors  section  provides  the  essential  path  and  agenda  item  flavors, 
and  their  associated  method  and  support  functions. 

The  A*  search  section  conducts  an  a*  search  of  the  volumes,  minimizing 
cost  and  visibility,  and  creates  an  initial  path. 

Finally,  the  optimization  code  optimizes  the  initial  A*  path  according 
to  snells  law  criteria.  This  section  may  create  one  or  several  paths 


(defvar  *PD-threshold*  ’0.0)  ;  maximum  desirable  probability  of  detection 

(defvar  *PD-modifier*  ’1 0.0)  ;  affects  effect  of  PD  on  path  planning 

(defvar  Tl*  ’3.14159) 


(defvar  ‘path-counter*  ’0) 
(defvar  *list-of-paths*  ’nil) 
(defvar  'agenda-counter*  ’0) 


;  path  name  variables 
;  location  of  ail  instanciated  paths 
;  agenda  instanciations 


(defvar  *Turn45*  ’10.0) 
(defvar  *Turn90*  ’50.0) 
(defvar  'BigTurn*  ’5000.0) 


;  cost  for  turn  of  45  degrees  or  less 
;  cost  for  turn  between  45  and  90  degrees 
;  cost  for  turns  greater  than  90  degrees 


(defvar  'Shallow-Climb*  ’1.2  ;  ratio  modifier  for  a  shallow  climb 
(defvar  'Steep-Climb*  ’1 .80)  ;  ratio  modifier  for  a  steep  climb 
(defvar  'Dive*  ’0.80)  ;  ratio  modifier  for  any  dive 


FLAVORS,  METHODS,  AND  FUNCTIONS 


PATH  FLAVOR 


(defflavor  path 
(start-point 
end-point 
volumes 
facets 


;goal 

;  general  path  "corridor" 
:  "windows"  in  cooridor 
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lines 

;  specific  path  to  follow 

points 

;  turn  points  in  path 

length 

;  of  current  lines 

total- K 

;  sum  of  deviations  from  snells  law  for  path 

max-detection-probability 

ave-detection-probability)  ;  average  of  entire  path  corridor 
(graphic) 

:gettable-instance-variables 
;settable-instance- variables 
:inittable-instance-variables 
:outside-accessible-instance-variables) 

; - METHODS  FOR  PATHS - 

(defmethod  (path  ilength)  ()  ;  find  the  total  length  of  the  path 

(let  ((val  ’0.0)) 

(cond  ((null  length) 

(loop  for  L  in  lines 

do  (setf  val  (+  val  (send  (eval  L)  ilength)))) 

(setf  length  val))) 
length)) 

(defmethod  (path  imax-detection-probability)  ()  ;  find  the  highest  PD  on  the  path 
(let  ((maximum  (voiume-probability-of-detection  (eval  (first  volumes))))) 

(loop  for  V  in  (rest  volumes) 

do  (cond  ((<  maximum  (volume-probability-of-detection  (eval  V))) 

(setf  maximum  (volume-probability-of-detection  (eval  V)))))) 

(setf  max-detection-probability  maximum))) 

(defmethod  (path  lave-detection-probability)  ()  ;  find  the  weighted  average  of  the  PD’s 
(let  ((weighted-sum  ’0.0)) 

(loop  for  Counter  from  0  to  (1  -  (length  volumes)) 
do  (setf  weighted-sum 

(+  weighted-sum 

(*  (send  (eval  (nth  Counter  lines))  ilength) 
(volume-probability-of-detection  (eval  (nth  Counter  Volumes))))))) 
(setf  ave-detection-probability  (/  weighted-sum 

(send  self  ilength))) 

ave-detection-probability)) 

(defmethod  (path  i  make-node-list)  ()  ;  used  by  graphic  mixin-flavor  to  draw 

(loop  for  P  in  points 

collect  (reverse  (append  (list  ’1 )  (reverse  (send  (eval  P)  ilist-format)))))) 


(defmethod  (path  imake-polygon-list)  ()  ;  used  by  graphic  mixin-flavor  to  draw 
(loop  for  L  in  lines 

do  (setf  Pti  (car  (send  (eval  L)  lendpolnt-list))) 
do  (setf  Pt2  (cadr  (send  (eval  L)  lendpoint-list))) 
collect  (list  (position-if  ’(lambda  (A)  (equal  A  R1 ))  node-list) 
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(position-if  ’(lambda  (A)  (equal  A  R2))  node-list)))) 


PATH  NAMES 


(defun  make-path-name  ()  ;  make  a  new  name  for  a  path 

(gensym  (incf  *path-counter*)) 

(intern  (gensym  "path"))) 


(defun  init-new-path  (start  end  volumes  facets  lines  points  length  K)  ;make  a  new  path 
(let  ((name  (make-path-name))) 

(set  name  (make-instance  'path 

:start-point  start 
:end-point  end 
ivolumes  voiumes 
rfacets  facets 
:lines  iines 
ipoints  points 
:length  length 
:total-K  K 

:max-detection-probability  ’nil 
:ave-detection-probability  ’nil)) 

(push  name  *list-of-paths") 
name)) 


AGENDA-ITEM  FLAVOR 


(defflavor  agenda-item 
(volume 
cost 

evaluation 

path) 

0 

:gettable-instance- variables 
:settable-instance- variables 
linittable-instance- variables 
:outside- accessible-instance- variables) 


; . AGENDA-ITEM  NAMES . 

(defun  make-agenda-item-name  () 

(gensym  (incf  'agenda-counter')) 

(intern  (gensym  "agenda"))) 

(defun  init-agenda-item  (volume  cost  evaluation  path) 


173 


(let  ((name  (make-agenda-item-name))) 

(set  name  (make-instance  'agenda-Hem 
:volume  volume 
;cost  cost 

revaluation  evaluation 


name)) 


:path  path)) 


SEARCHE  S 


A*  Search 


(defun  A-star-search  (Start-point  End-point  Trace-flag) 

(let*  ((start-volume  (first  (locate-point-air  start-point))) 

(goal-volume  (first  (locate-point-air  end-point))) 

(successor-volumes  (volume-connected-to  (eval  start-volume))) 
(path-volumes  ’nil) 

(agenda  ’nil) 

(best-path)) 

(terpri)  (terpri) 

(princ  "»»Begin  A-star  Search")  (terpri)  (terpri) 

(princ "  Start  Volume: ")  (print  start-volume)  (terpri) 

(princ  "  Goal  Volume: ")  (print  goal-volume)  (terpri)  (terpri) 

(cond  (trace-flag 

(terpri)  (princ  "Search  trace  selected.  Top  five  and  bottom  five  items") 
(terpri)  (princ  "on  seach  agenda  will  be  printed.")  (terpri)  (terpri)) 

(t  (terpri))) 

(princ  "Search") 

;  initalize  the  search  agenda 

(setf  agenda  (put-successors-on-agenda 

start-volume  ;  end  of  last  path 

successor-volumes  ;  successors  to  be  added 

’0.0  ;  cost 

(list  start-volume)  ;  path  to  date 

end-point  ;  goal 

agenda))  ;  agenda  to  be  changed 

;  SEARCH  along  best  agenda  item  for  all  possible  paths 
;  until  get  to  the  goal  along  one  of  the  paths 
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(loop  until  (goal-on-agenda-p  goal-volume  agenda) 
do  (princ 

do  (cond  (trace-flag 

(princ " - New  Agenda . . ") 

(print-agenda  agenda))) 
do  (let*  ((best-successor-volume  (first  agenda)) 

(successors-to-best  (volume-connected-to  (aval  (agenda-item-volume 

(eval  best-successor-volume))))) ) 
(sett  successors-to-best  (remove  ’EDGE  successors-to-best)) 

(loop  for  V  in  (rest  (agenda-item-path  (eval  best-successor-volume))) 
do  (setf  successors-to-best  (remove  V  successors-to-best))) 

(sett  agenda  (remove  best-successor-volume  agenda)) 

(setf  agenda  (put-successors-on-agenda 

(agenda-item-volume  (eval  best-successor-volume)) 
successors-to-best 

(agenda-item-cost  (eval  best-successor- volume)) 
(agenda-item-path  (eval  best-successor-volume)) 
end-point 
agenda)))) 

:  SEARCH  COMPLETED! 

:  find  lines  and  points  in  search 

(setf  path-volumes  (reverse  (find-path  goal-volume  agenda)))  :get  resultant  path 
(setf  best-path  (init-new-path  start-point 

end-point 

path-volumes 

’nil 

'nil 

'nii 

'nil 

’nil)) 

(princ  "Completed")  (terpri)  (terpri) 

(make-facet-to-facet-path  best-path)  ;  make  initial  guess  at  optimal  path 
(calc-path- and-stats  best-path)  ;  fill  out  rest  of  path  flavor  data 

best-path)) 


A*  Search  with  multiple  solutions 


(defun  A-star-search-M  (Start-point  End-point  Trace-flag  paths) 

(let*  ((start-volume  (first  (locate-point-air  start-point))) 

(goal-volume  (first  (locate-point-air  end-point))) 
(successor-volumes  (volume-connected-to  (eval  start-volume))) 
(path-volumes  ’nil) 

(agenda  ’nil) 
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(paths-found)) 

(terpri) 

(princ  "»»Begin  A-star  Search")  (terpri)  (terpri) 

(princ "  Start  Volume; ')  (print  start-volume)  (terpri) 

(princ "  Goal  Volume: ")  (print  goal-volume)  (terpri)  (terpri) 

(cond  (trace-flag 

(terpri)  (princ  "Search  trace  selected.  Top  five  and  bottom  five  items") 

(terpri)  (princ  "on  seach  agenda  will  be  printed.”)  (terpri)  (terpri)) 

(t  (terpri))) 

;  initalize  the  search  agenda 

(setf  agenda  (put-successors-on-agenda 

start-volume  ;  end  of  last  path 

successor-volumes  ;  successors  to  be  added 

'0.0  ;  cost 

(list  start-volume)  ;  path  to  date 

end-point  ;  goal 

agenda))  ;  agenda  to  be  changed 

;  SEARCH  along  best  agenda  item  for  all  possible  paths 
;  until  get  to  the  goal  along  one  of  the  paths 

(loop  repeat  paths  ;  find  top  several  paths 
do  (terpri) 
do  (princ  "Search") 

do  (loop  until  (goal-on-agenda-p  goal-volume  agenda) ;  same  loop  as  single  search 
do  (princ ".") 
do  (cond  (trace-flag 

(princ " . . New  Agenda- — . ") 

(print-agenda  agenda))) 
do  (let*  ((best-successor-volume  (first  agenda)) 

(successors-to-best  (volume-connected-to  (eval  (agenda-item-volume 

(eval  best-successor-volume)))))) 
(setf  successors-to-best  (remove  'EDGE  successors-to-best)) 

(loop  for  V  in  (rest 

(agenda-item-path  (eval  best-successor-volume))) 
do  (setf  successors-to-best  (remove  V  successors-to-best))) 

(setf  agenda  (remove  best-successor-volume  agenda)) 

(setf  agenda  (put-successors-on-agenda 

(agenda-item-volume  (eval  best-successor-volume)) 
successors-to-best 

(agenda-item-cost  (eval  best-successor- volume)) 
(agenda-item-path  (eval  best-successor- volume)) 
end-point 
agenda)))) 

(setf  path-volumes  (reverse  (find-path  goal-volume  agenda))) 

(setf  agenda  (remove-goal  goal-voiume  agenda)) 

(setf  paths-found  (adjoin  (init-new-path  start-point 

end-point 
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path-volumes 

’nil 

'nil 

'nil 

’nil 

’nil) 

paths-found)) 

(princ  "Completed")  (terpri)  (terpri) 

(make-facet-to-facet-path  (first  paths-found)) 
(calc-path-and-stats  (first  paths-found))) 
paths-found)) 


Search  utility  functions 


; - agenda  manipulations - 

;  for  A-STAR  search 

(defun  put-successors-on-agenda  (pred-volume 

successor-volumes 

cost  ;  cost  so  far 

path  ;  volumes 

goal 

agenda) 

(loop  for  V  in  successor-volumes 

do  (setf  agenda  (adjoin  (init-agenda-item  V  ;  name 

(+  cost  (cost-function  V  path)) 
(evaluation-function  pred-volume 

V 

path 

goal) 

(adjoin  V  path))  ;  path 

agenda))) 

(stable-sort  agenda  #’agenda-sort-p)) 

(defun  agenda-sort-p  (A  B) 

(cond  ((LT  (+  (agenda-item-cost  (eval  A)) 

(agenda-item-evaluation  (eval  A))) 

(+  (agenda-item-cost  (eval  B)) 

(agenda-item-evaluation  (eval  B)))) 

(return-from  agenda-sort-p ’t))) 

’nil) 

(defun  goal-on-agenda-p  (goal  agenda)  ;  return  T  iff  goal  volume  is  on  the  agenda 
(loop  for  A  in  agenda 

do  (cond  ((equal  goal  (agenda-item-volume  (eval  A))) 

(return-from  goal-on-agenda-p  ’T)))) 
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’nil) 


(defun  remove-goal  (goal  agenda) 

(loop  for  A  in  agenda 

do  (cond  ((equal  goal  (agenda-item-volume  (eval  A))) 

(return-from  remove-goal  (remove  A  agenda))))) 

’nil) 

(defun  find-path  (goal  agenda)  ;  get  the  path  once  the  goal  is  found 
(loop  for  A  in  agenda 

do  (cond  ((equai  goai  (agenda-item-voiume  (eval  A))) 

(return-from  find-path  (agenda-item-path  (eval  A))))))) 

(defun  print-agenda  (agenda)  ;  print  agenda  and  some/all  items  on  the  agenda 
(terpri)  (pprint  agenda)  (terpri) 

(cond  ((>=  10  (length  agenda)) 

(princ  "Entire  agenda: ")  (terpri)  ;  print  whole  agenda  if  short 
(loop  for  I  in  agenda 
do  (terpri) 
do  (describe  I))) 

(t  (princ  "First  five  in  agenda: ")  (terpri) ;  do  first  five  and  last  five 
(loop  for  Count  in  ’(0  1  2  3  4)  ;  if  long 

do  (describe  (nth  count  agenda)) 
do  (terpri)) 

(terpri)  (princ  "Last  five  on  agenda: ")  (terpri) 

(loop  for  Count  in  ’(6  5  4  3  2  1 ) 
do  (describe  (nth  (-  Count  (length  agenda))  agenda)) 
do  (terpri)))) 

(terpri)  (terpri)) 


: — evaluation  and  cost  functions - 

(defun  evaluation-function  (VP  VS  path-volumes  Goal) 

(let  ((turn-modifier  (eval-turn-cost  VP  VS  path- volumes)) 

(altitude-modifier  (eval-climb-dive  VP  VS)) 

(PD-modifier  (+  ’1 .0  (*  *PD-modifier* 

(-  (volume-probability-of-detection  (eval  VS)) 
TD-threshold*)))) 

(basis-distance  (distance  (volume-arithmetic-center  (eval  VS))  Goal))) 

(*  PD-modifier  (*  altitude-modifier  (+  turn-modifier  basis-distance))))) 

(defun  cost-function  (VS  path-volumes) 

(let  ((altitude-modifier  (eval-climb-dive  (first  path-volumes)  VS)) 

(turn-modifier  (eval-tum-cost  (first  path-volumes)  VS  path-volumes)) 
(PD-modifier  ’0.0) 

(basis-cost  (distance  (volume-arithmetic-center  (eval  VS)) 

(volume-arithmetic-center  (eval  (first  path-volumes)))))) 

(loop  for  V  in  path-volumes 
do  (sett  PD-modifier  {+  PD-modifier 

(volume-probability-of-detection  (eval  V))))) 
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(self  PD-modifier  (/  PD-modifier  (length  path-volumes))) 

(sett  PD-modifier  (+  '1.0  (*  *PD-modifier* 

(-  PD-modifier  *PD-threshold*)))) 
(*  PD-modifier  (*  altitude-modifier  (+  turn-modifier  basis-cost))))) 


(defun  eval-turn-cost  (VP  VS  Path-volumes) 

(let  ((projected- VP-center  (project-xy  (volume-arithmetic-center  (eval  VP)))) 

(projected- VS-center  (project-xy  (volume-arithmetic-center  (eval  VS)))) 
(previous-volume  (find-previous-volume  VP  Path-volumes)) 

(projected-vol-center  ’nil) 

(path  ’nil) 

(new-path  ’nil) 

(angle-of-turn  ’nil)) 

(cond  ((equal  VP  previous-volume)  ;  no  previous  path  ? 

(retum-from  eval-turn-cost  ’1 .0)) 

(t  (setf  projected-vol-center  (project-xy 

(volume-arithmetic-center  (eval  previous-volume)))) 
(setf  path  (make-line  projected-vol-center  projected-VP-center)) 

(setf  new-path  (make-line  projected-VP-center  projected-VS-center)) 

(setf  angle-of-turn  (angle-between-lines  path  new-path)) 

(cond  ((null  angle-of-tum) 

(return-from  eval-turn-cost  ’0.0)))  ;  no  turn 

(cond  ((GT  (/  *PI*  ’4.0)  angle-of-turn) 

(return-from  eval-turn-cost  *Turn45*)))  ;  turn  <  45  degrees 
(cond  ((GT  (/  *PI*  ’2.0)  angle-of-turn) 

(return-from  eval-turn-cost  *Tum90*)))))  ;  90  <  turn  <  45 
*BigTurn*))  ;  turn  >  90 


(defun  project-xy  (Point) 

(let  ((point-coords  (send  (eval  Point)  :list-format))) 

(init-point  (list  (first  point-coords)  (second  point-coords)  ’0.0)))) 

(defun  find-previous-volume  (VP  path-volume) 

(let  ((position-VP  (position  VP  path-volume))) 

(cond  ((>  1  (length  path-volume)) 

(return-from  find-previous-volume  (elt  (1+  position-VP)  path-volume))) 

(t  (return-from  find-previous-volume  (first  path-volume)))))) 

(defun  eval-climb-dive  (VP  VS) 

(let*  ((inter-facet  (find-common-facet  VP  VS)) 

(interfacet-z  (third  (mean-point-in-facet  inter-facet))) 

(path-z  (third 

(send  (eval  (volume-arithmetic-center  (eval  VP)))  :list-format)))) 
(cond  ((and  (LT  path-z  (*  interfacet-z  ’1.10)) 

(GT  path-z  (*  interfacet-z  ’0.90))) 

(return-from  eval-dimb-dive  ’1 .0))  ;  level  flight 

((GT  path-z  interfacet-z) 

(return-from  eval-climb-dive  ‘Dive*))  ;  dive 
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(t  (loop  for  P  in  (send  (eval  inter-facet)  ipoints) 

do  (cond  ((>  path-z  (third  (send  (eval  P)  :lfst-fonnal))) 

;  shallow  dimb 

(return-from  eval-climb-dive  *Shallow-Climb‘)))))) 
•Steep-Climb*))  ;  steep  dimb 


; — general  functions  in  support  of  path  planning - 


(defun  Calc-path-and-stats  (path)  ;  used  to  find  support  info  on  a  new  path 
(send  (eval  path)  :length) 

;  determine  probability  limits 

(send  (eval  path)  :max-detection-probability) 

(send  (eval  path)  :ave-detection-probability) 

(princ  "»»Path  Statistics:*)  (terpri)  (terpri) 

(princ "  MaxiiTium  detedion  probability: ") 

(print  (path-max-detection-probability  (aval  path))) 

(terpri) 

(princ "  Average  detection  probability:  *) 

(print  (path-ave-detection-probability  (eval  path))) 

(terpri) 

(princ  "  Total  length  of  path: ") 

(print  (path-length  (eval  path))) 

(terpri) 

(princ "  Total  number  of  maneuvers: ")  (print 

(-  (length  (path-points  (eval  path)))  ’2)) 

(terpri)  (terpri) 

(princ  "»»Path: ")  (print  path)  (terpri)  (terpri) 

’nil) 

(defun  find-intermediate-facets  (path)  ;  find  all  the  facets  along 

;  the  path 

(let  ((previous-volume  (first  (path-volumes  (eval  path)))) 

(facets  ’nil)) 

(loop  for  V  in  (rest  (path-volumes  (eval  path))) 
do  (setf  facets  (adjoin  (find-common-facet  previous-volume  V)  facets)) 
do  (setf  previous-volume  V)) 

(reverse  facets))) 

(defun  make-facet-to-facet-path  (path) 

(let  ((last-point  (path-start-point  (eval  path))) 

(points  (path-start-point  (eval  path))) 

(lines  ’nil)) 

(setf  (path-facets  (eval  path))  (find-intermediate-facets  path)) 

(loop  for  F  in  (path-facets  (eval  path)) 
do  (let  ((next-point  (init-point  (mean-point-in-facet  F)))) 

(setf  lines  (adjoin  (make-line  last-point  next-point)  lines)) 
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(setf  points  (adjoin  next-point  points)) 

(sett  last-point  next-point))) 

(push  (make-line  last-point  (path-end-point  (eval  path)))  lines) 

(push  (path-end-point  (eval  path))  points) 

(setf  (path-lines  (eval  path))  (reverse  lines)) 

(sett  (path-points  (eval  path))  (adjoin  (path-start-point  (eval  path))  (reverse  points))))) 


PATH  OPTIMIZATION 


OPTIMIZE  PATH  ACCORDING  TO  SNELL’S  UVW.  D.H.  LEWIS  10/11/88 

Develop  an  expression  for  sneli's  constant  at  each  facet  along  the 
the  path,  and  then  minimize  it  with  respect  to  the  facets  before 
and  after  the  facet  concerned.  Sum  all  constants  along  the  path 
to  determine  the  net  amount  of  deviation  from  sneli’s  law.  Repeat 
until  total  constant  minimized. 


(defvar  *PI2*  (/  *PI*  ’2.0)) 

(defvar  ‘search-increment*  ’10) 

: . MAIN  PATH  OPTIMIZATION  FUNCTION - 

(defun  optimize-path  (path) 

(let  ((new-path-poin‘s  (list  (path-start-point  (eval  path)))) 

(new-path-lines  ’nil) 

(new-path-length  ’0.0) 

(last-point  ’nil)  ;  dummy  for  building  path  lines 
(total-K  ’0.0))  :  total  deviation  from  sneli’s  law 

:  optimize  path  point  for  each  facet  in  turn, 

;  appending  new  points  onto  new-point  list  as 
;  they  are  created 

(terpri)  (terpri) 

(princ  "Optimizing  path  ')  (print  path)  (princ ":’)  (terpri)  (terpri) 

(loop  for  Facet-nr  from  '1  to  (length  (path-facets  (eval  path))) 
do  (let  0 

(princ  "Optimizing  at  facet  number ") 

(print  facet-nr)  (princ " : ")  (print  (nth  (1-  facet-nr)  (path-facets  (eval  path)))) 
(terpri)) 

do  (let  ((prev-point  ’nil) 

(next-point  (nth  (1-t-  facet-nr)  (path-points  (eval  path)))) 

(path-point  (nth  facet-nr  (path-points  (eval  pa’!')))) 
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(new-point  ’nil) 

(facet  (nth  (1-  facet-nr)  (path-facets  (eval  path)))) 

(N1  (-H  '1  (volume-probability-of-detection 

(eval  (nth  (1-  facet-nr)  (path-volumes  (eval  path))))))) 
(N2  (+  ’1  (volume-probability-of-detection 

(eval  (nth  facet-nr  (path-volumes  (eval  path)))))))) 

;  use  ’best"  previous  point  estimate 

(cond  ((>  facet-nr ’1) 

(setf  prev-point  (first  new-path-points))) 

(t  (setf  prev-point  (nth  (1-  facet-nr)  (path-points  (eval  path)))))) 

(pprint  (list  ’"initial: "  facet-nr  prev-point  path-point  next-point  facet  N1  N2)) 
(setf  new-point  (optimize-point-on-facet  prev-point 

next-point 

facet 

path-point 

N1 

N2)) 

(pprint  (list  ’"new  path  point: "  new-point  (get  new-point  ’K))) 

(setf  new-path-points  (adjoin  new-point  new-path-points)) 

(setf  total-K  (+  total-K  (get  new-point  ’K))))) 

;  add  goal  to  new  points,  draw  new  path 

(setf  new-path-points  (adjoin  (car  (last  (path-points  (eval  Path))))  new-path-points)) 
(setf  new-path-points  (reverse  new-path-points)) 

(setf  last-point  (first  new-path-points)) 

(loop  for  P  in  (rest  new-path-points) 
do  (let  0 

(setf  new-path-lines  (adjoin  (make-line  last-point  P)  new-path-lines)) 

(setf  new-path-length  (+  (send  (eval  (first  new-path-lines))  :length) 

new-path-length)) 

(setf  last-point  P))) 

(setf  new-path-lines  (reverse  new-path-lines)) 

;  build  the  new  path  with  optimized  path  data 


(terpri)  (terpri) 

(princ  "Optimization  completed")  (terpri) 

(calc-path-and-stats  (init-new-path  (path-start-point  (eval  path)) 

(path-end-point  (eval  path)) 

(path-volumes  (eval  path)) 

(path-facets  (eval  path)) 

new-path-lines 

new-path-points 

new-path-length 

total-K)))) 
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FIND  THE  BEST  POINT  ON  THE  FACET- 


(defun  optimize-point-on-facet  (prev-point  next-point  facet  path-point  N1  N2) 

;  Find  the  point  on  the  facet  with  the  lowest 
;  snell’s  constant  (K). 

(let*  ((straight-path-line  (make-line  prev-point  next-point)) 

(straight-path-point  (find-interoept-point  facet  straight-path-line)) 

(path-K-line  (make-line  path-point  straight-path-point)) 

(path-plane  (make-a-plane  prev-point  path-K-line)) 

(list-of-points  (find-edge-points-of-facet  path-plane  facet))) 

;  (pprint  list-of-points) 

;  (pprint  (list  facet  straight-path-point)) 

(setf  (get  straight-path-point  ’K)  (find-snells-constant 

slraight-path-point 

(make-line  straight-path- point  prev-point) 

(make-line  straight-path- point  next-point) 

facet 

N1 

N2)) 


;  do  special  cases  first 

(cond  ((inside-facet-p  straight-path-point  facet) 

(cond  ((equal  ’0.0  (*  '1.0  (get  straight-path-point  ’K))) 

(retum-from  optimize-point-on-facet  straighf-path-point)) 

(t  (setf  list-of-points  (adjoin  straight-path-point  list-of-points))))) 

(t  (setf  list-of-points  (adjoin  path-point  list-of-points)))) 

;  (pprint  (list  list-of-points  (length  list-of-points))) 

(cond  ((<  '1  (length  list-of-points)) 

(setf  path-point  (optimize-K-on-line  list-of-points 

prev-point 

next-point 

facet 

N1 

N2))) 

(t  (setf  (get  path-point  ’K)  (find-snells-constant  Path-point 

(make-line  Path-point  prev-point) 
(make-line  Path-point  next-point) 
facet 
N1 

N2)))) 

path-point)) 


(defun  optimize-K-on-line  (agenda  prev-point  next-point  facet  N1  N2) 
(let  ((lowest-K-point  ’nil) 


183 


(best-line  ’nil) 

(mid-point  ’nil)) 

;  (pprint  (list  ’"Optimize: "  agenda)) 

(loop  for  P  in  agenda 
do  (sett  (get  P  ’K)  (find-snells-constant  P 

(make-iine  P  prev-point) 

(make-line  P  next-point) 

facet 

N1 

N2))) 

(setf  agenda  (stable-sort  agenda  #’agenda-sort-on-K)) 

(sett  lowest-K-point  (first  agenda)) 

;  (pprint  (list  ’"Sorted  optimize: "  agenda  lowest-K-point)) 

(loop  repeat  ’3 
do  (let  0 

(setf  best-line  (make-line  (first  agenda)  (second  agenda))) 

(sett  mid-point  (init-point  (send  (eval  best-line)  :midpoint))) 

(setf  (get  mid-point  ’K)  (find-snells-constant  mid-point 

(make-line  mid-point  prev-point) 

(make-line  mid-point  next-point) 

facet 

N1 

N2)) 

(setf  agenda 

(stable-sort  (list  (first  agenda)  (second  agenda)  mid-point) 
#'agenda-sort-on-K)) 

;  (pprint  agenda) 

;  (pprint  (iist  (first  agenda)  (get  (first  agenda)  ’K))) 

)) 


(first  agenda))) 


(defun  find-edge-points-of-facet  (plane  facet) 

(let  ((intercept-points  ’nil)) 

(loop  for  E  in  (facet-edges  (eval  facet)) 
do  (let  ((intercept-point  (find-intercept-point  plane  E))) 

(cond  ((not  (null  intercept-point)) 

(setf  intercept-points  (adjoin  intercept-point  intercept-points)))))) 

intercept-points)) 


(defun  agenda-sort-on-K  (A  B)  ;  sort  by  increasing  absolute  value  of  K  property 
(<  (abs  (get  A  ’K))  (abs  (get  B  ’K)))) 


FIND  SNELLS  CONSTANT 
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(defun  find-snells-constant  (Point  Line- 1  Line-2  Facet  N1  N2) 

;  find  sneiis  constant  at  a  boundary,  i.e.: 

K  *  N1  *  sin(theta-1)  -  N2  *  sin(theta-2) 

;  note;  returns  NIL  if  anything  would  "blow  this  up" 


(let*  ((end-point-normal-line 

(inif-point  (map  'list  ’+  (send  (eval  Point)  .list-format) 

(map ’list’*  ’(100100  100) 

(send  (eval  facet)  :list-coeff-3))))) 

(normal-line  (make-line  Point  end-point-normal-line)) 

(perpendicular-plane 

(make-a-plane 

(init-point  (list  '0  '0  (third  (send  (eval  point)  :list-format)))) 
normal-line)) 

(line-joining-points  (make-line  (send  (eval  line-1)  :end-point) 

(send  (eval  line-2)  :end-point))) 

(default ’100) 

(theta-1  (angle-between-lines  Line-1  normal-line)) 

(theta-2  (angle-between-lines  Line-2  normal-line))) 

(cond  ((and  (not  (null  Theta-1)) 

(not  (null  theta-2))) 

(setf  theta-1  (abs  (realpart  theta-1))) 

(self  theta-2  (abs  (realpart  theta-2))) 

(cond  ((<  *PI2*  theta-1 ) 

(setf  theta-1  (-  *PI*  theta-1 )))) 

(cond  ((<  *PI2*  theta-2) 

(setf  theta-2  (-  *PI*  theta-2)))) 

(cond  ((>  theta- 1  (realpart  (asin  (/  N2  N1))))  ;  critical  angle? 

(setf  theta-2  *PI2*))) 

(cond  ((send  (eval  line-joining-points)  :strattle-plane-p  perpendicular-plane) 
(return-from 

find-snells-constant  (-  (*  N1  (sin  theta- 1)) 

(*  N2  (sin  theta-2))))) 

(t  (return-from 

find-snells-constant  (-  (*  N1  (sin  theta-1)) 

(*  N2  (-  (*  ’2  ’PI*) 


default)) 


(sin  theta-1))))))))) 
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FILE  NAME:  Common-functions . lisp 


;  Mode: Common- Lisp;  BaseilO 


COMMON  FUNCTIONS 


This  file  consists  of  all  common  functions  used  by  most  of  the 
files  of  the  3-D  path  planning  software.  Function  vary  from  the 
very  general  (convenience)  functions,  to  very  detailed,  special 
purpose  functions  (which  happen  to  be  called  from  two  seperate 
files).  Functions  are  grouped  by  categories  of  Simple  functions. 
Point  functions,  Vector  functions.  Line  functions.  Plane  functions, 
Facet  functions.  Volume  functions,  property  list  functions, 
detailed  (special  purpose)  functions,  and  finally,  printing  functions. 

D.H. Lewis/Thesis  07  AUG  88 


DIRECTORY  OF  FUNCTIONS 


SIMPLE;  MEMBER-P 
EQUAL-P 
EQUAL-ZERO-P 
DISTANCE 
MERQE-JOIN-LIST 
FIRST-NON-ZERO 
EQUAL-ERROR 
LT,  GT,  GE,  LE 


POINTS:  AVERAGE-OF-POINTS 
FIND-POINT 
AVERAGE-POINT 


VECTORS:  SOLVE-FOR-T 

VECTOR-ADD-WITH-T 


LINES:  MAKE-LINE 


PLANES;  MAKE-A-PLANE 


LINE-CROSS-PRODUCT 

FIND-COMMON-POINT 

ANGLE-BETWEEN-LINES 

FACETS:  FIND-COMMON-FACET 
MEAN-POINT-IN-A-FACET 
MEAN-POINT-IN-A-FACET-2 
INFO-ON-FACETS 
INSIDE-FACET-P 


MAKE-A-NORMALIZED-PLANE 

MAKE-VERTICAL-PLANE 

MAKE-Z-PLANE 

MAKE-X-PLANE 

MAKE-Y-PLANE 

SUBS-POINT-INTO-EQUATION 

SUBS-LINE-INTO-PLANE-EQUATION 


VOLUMES;  INTERSECT-ALL-PLANES-WITH- VOLUMES 
PROPERTY  LISTS;  RESET-POINT-PROPERTY-LISTS 


DETAILED  FUNCT.ONS:  MINIMUM-DISTANCE 


LOCATE-POINT-AIR 

POINT-IN-VOLUME-P 

POINT-CHECK-P 

LINES-STRATTLE-FACETS-P 

SPEED-DEMON 

PRINTING  FUNCTIONS;  DUMP-VOLUMES 
DDUMP-PATH 
PRINT-POINTS 
PRINT-VECTORS 
PRINT-LINES 
PRINT-FACETS 
PRINT-VOLUMES 


(defvar  ‘precision*  ’0.0025) 

; - SIMPLE  FUNCTIONS 


(defun  member-p  (Hem  list)  ;  T  or  nil  member 

(not  (null  (member  item  list)))) 

(defun  equal-p  (listi  Iist2)  ;  are  two  lists  equal? 

(cond  ((equal  (length  listi)  (length  Iist2)) 

(apply  ’and  (mapcar  ’equal  listi  Iist2))))) 

(defun  equal-zero-p  (A)  ;  is  A  equal  to  zero? 

(cond  ((equal  (*  ’1.0  A)  ’0.0) 

(retum-from  equal-zero-p ’t))) 

’nil) 

(defun  distance  (PI  P2)  ;  distance  between  two  points 

(let  ((difference  (mapcar  ’-  (send  (eval  P1 )  :list-format) 

(send  (eval  P2)  ;llst-format)))) 

(sqrt  (apply  ’+  (mapcar  ’*  difference  difference))))) 

(defun  merge-join-list  (Listi  List2)  ;  join  the  two  fists  to  make 

(let  ((lengthi  (length  listi ))  ;  one  long  list 

(tength2  (length  Iist2)) 

(templist  'nil)) 

(cond  ((>=  lengthi  Iength2) 

(setf  templist  listi) 

(loop  for  i  in  Iist2 

do  (setf  templist  (adjoin  I  templist)))) 

(t  (setf  templist  Ilst2) 

(loop  for  I  in  listi 

do  (setf  templist  (adjoin  I  templist))))) 

templist)) 
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(defun  first-non-zero  (List)  ;  find  the  first  non-zero  element  in  a  simple  list 
;  If  none  found,  return  "-1". 

(cond  ((not  (equal-zero-p  (first  List))) 

(first  List)) 

((not  (equal-zero-p  (second  List))) 

(second  List)) 

((not  (equal-zero-p  (third  List))) 

(third  Ust)) 

(t(-1)))) 

(detun  equal-error  (A  B)  ;  equal  within  an  allowed  level  of  error 

(let  ((error  ’nil)) 

(cond  ((equal  A  B)  ;  simple  equal 

(return-from  equal-error ’t)) 

((equal  (*  '1 .0  A)  ;  floating  point  equal 

r  ’1.0  B)) 

(retum-from  equal-error ’t)) 

((or  (equal-zero-p  B)  ;  divide  by  zero  check 
(equal-zero-p  A)) 

(setf  error  '1 .0)) 

((>  A  B)  ;  find  absolute  error  between  terms 

(setf  error  (abs  (/  (-  A  B)  B)))) 

(t  (setf  error  (abs  (/  (-  A  B)  A))))) 

(<=  error  'precision*)))  ;  check  with  allowed  precision 

(defun  LT  (A  B) 

(and  (not  (equal-error  A  B)) 

(<A8))) 

(defun  GT  (A  B) 

(and  (not  (equal-error  A  B)) 

(>AB))) 

(defun  LE  (A  B) 

(not  (GT  A  B))) 

(defun  GE  (A  B) 

(not  (LT  A  B))) 

; - MANIPULATE  POINTS - 

(defun  average-of-points  (llst-of-points) 

(map  list  '(lambda  (a  b)  (/  a  b))  (mean-point-in-facet-2  list-of-points) 
(make-list  3  ;initial-element 

(length  list-of-points)))) 

(defun  find-point  (X  Y  Z  List-of-points)  ;  find  all  points  in  list  which  match 
(let  ((result  List-of-points)  ;  one  or  more  of  specified  values,  values 
(values  (list  X  Y  Z)))  ;  of  'nil  will  be  ignored,  returns  a  list. 

(loop  for  Pass  In  (List  0  1  2) 
do  (cond  ((not  (equal  'nil  (nth  Pass  values))) 
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(let  ((intermediate-result  ’nil)) 

(loop  for  P  in  result 

do  (cond  ((equal-error  (nth  Pass  values) 

(nth  Pass  (send  (eval  P)  ;list-format))) 
(setf  intermediate-result 

(adjoin  P  intermediate-resuit))))) 

(setf  result  intermediate-result))))) 

result)) 

(defun  average-points  (Pt1  Pt2) ;  find  the  point  1/2  way  between  two  points 
(map  'list  ’/  (map  'iist '+  (send  (evai  Pti )  Jist-format) 

(send  (eval  Pt2)  :list-format)) 

(make-list  3  :initial-element  ’2))) 


; - make  or  MANiPULATE  VECTORS - 

(defun  solve-for-t  (Plane  Line  Denom) 

(/  (-  (fourth  Piane)  (apply  ’+  (map  'list  ’*  Piane 

(send  (eval(Line-segment-position-vector 

(eval  Line)))  :list-format))))  Denom)) 

(defun  vector-add-with-t  (DV  PV  Ti)  ;  add  a  direction  vector  (*T)  to  a  position  vector 
(map  'iist  ’+  (send  (evai  PV)  :list-format) 

(map  iist  #’(iambda  (A)  (*  A  Ti))  (send  (eval  DV)  :list-format)))) 


MAKE  OR  MANIPULATE  LINES - 


(defun  make-line  (Pointi  Point2) 

(init-line  (init-vector  ’*origin*  Pointi) 

(init-vector  Pointi  Point2))) 

(defun  line-cross-product  (LI  L2)  ;  take  the  cross  product  of  direction  vectors 
(cross-product  (send  (eval  (line-segment-direction-vector  (evai  LI )))  list-format) 
(send  (eval  (line-segment-direction-vector  (eval  L2)))  :iist-format)) 


(defun  find-common-point  (LI  L2)  ;  returns  the  value  of  a  common  point, 

(loop  for  m  in  (send  (eval  LI)  .endpoints)  ;  If  one  exists, 
do  (loop  for  n  in  (send  (eval  L2)  :endpoints) 
when  (equal  m  n) 

do  (retum-from  find-common-point  m))) 

’nil) 

(defun  angle-between-lines  (LI  L2)  ;  find  the  smallest  angle  between  two  lines 
;  return  NIL  for  unusual  problems 
(let*  ((normal- vector  (line-cross-product  LI  L2)) 
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(normal-vector-tengtfi  (sqrt  (abs  (+  (*  (first  normal-vector) 

(first  normal-vector)) 

(*  (second  normal-vector) 
(second  normal-vector)) 

(•  (third  normal-vector) 

(third  normal-vector))))))) 

(cond  ((equal-zero-p  normal-veclor-length) 

(retum-from  angle-between-llnes  ’nil)) 

((or  (equal-zero-p  (send  (eval  LI )  :length)) 

(equal-zero-p  (send  (eval  12)  .length))) 

(retum-from  angle-between-llnes  ’nil))) 

(-  *Pr  (asin  (/ normal-vector-length  (*  (send  (eval  LI)  ilength) 

(send  (eval  L2)  .length))))))) 


(detun  find-mid-point  (Line) 
(send  (eval  Line)  :midpoint)) 


; - make  or  MANIPULATE  PLANES - 

(defun  make-a-plane  (point  line)  ;  define  a  plane  given  a  point  and  a  line 
(let*  ((Obs-line  (init-line  (init-vector  "origin*  point) 

(init-vector  point 

(first  (send  (eval  line)  :endpoints))))) 
(plane  (make-a-normalized-plane  Obs-line  line))) 

(init-plane  plane))) 


(defun  make-a-normalized-plane  (LI  L2)  ;  make  a  plane  equation  with 

;  Ao  =  -1,0,1 :  first  coef  is  positive 

(let  ((un-normalized  (line-cross-product  LI  L2))  ;  normal  vector  to  plane 
(common-point  (find-oommon-point  LI  L2)) 

;  a  point  In  the  plane 

(Ao  ’nil)  ;  constant  in  plane  equation 

(normalized  ’nil))  ;  in  standard  form 

(setf  un-normalized  (map  ’list  ’rationalize  un-normalized)) 

(cond  ((null  common-point) 

(setf  common-point  (send  (eval  (send  (eval  LI )  :start-polnt))  .list-format))) 

(t  (setf  common-point  (send  (eval  common-point)  ;llst-format)))) 

(setf  Ao  (apply  ’+  (mapcar  ’*  common-point  un-normalized))) 

(cond  ((equal-zero-p  Ao) 

(setf  normalized 

(map  'list  ’/  un-normalized  (make-list  3  :lnitlal-element 

(ftrst-non-zero  un-normalized)))) 

(setf  normalized  (reverse  (append  (list  ’0)  (reverse  normalized))))) 

(t  (setf  normalized 

(map  ’list  ’/  un-normalized  (make-list  3  initial-element  Ao))) 

(setf  normalized  (reverse  (append  (list  ’1)  (reverse  normalized)))))) 

(cond  ((QT  ’0.0  (first-non-zero  normalized)) 
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(map  'list  '*  (make-list  4  :initial-element  (- 1))  normalized)) 
(ft)) 

(sett  normalized  (map  'list  'rationalize  normalized)) 
normalized))  ;  return  the  coeffs  for  the  plane 


(defun  make-vertical-plane  (Line) 

(let*  ((line-endpoints  (send  (eval  Line)  :endpoints)) 

(Pti  (map  'list '+  '(0  0 10) 

(send  (eval  (first  line-endpoInts))  ;list-format))) 
(LI  (make-Kne  (init-point  Pti )  (second  line-endpoints))) 

(L2  (make-line  (init-point  Pti )  (first  tine-endpoints)))) 

(init-plane  (make-a-normalized-piane  LI  L2)))) 


(defun  make-z-plane  (point) 

(init-piane  (make-a-normaiized-plane 

(make-line  (init-point 

(map  'list '+  (send  (eval  point )  ;list-format) 
'(10  0  0))) 


point) 

(make-line  (init-point 

(map  'list  ’+  (send  (eval  point) ;  list-format) 
'(0  10  0))) 

point)))) 

(defun  make-y-plane  (point) 

(init-plane  (make-a-normalized-plane 

(make-line  (init-point 

(mj^  'list '+  (send  (eval  point )  -.list-format) 
'(0  010))) 

point) 

(make-line  (init-point 

(ma^J  'list '+  (send  (eval  point) :  list-format) 
'(0  10  0))) 

point)))) 


(defun  make-x-plane  (point) 

(init-plane  (make-a-normalized-plane 

(make-line  (inH-point 

(map  'list '+  (send  (eval  point )  ;iist-format) 
'(10  0  0))) 

point) 

(make-line  (init-point 

(map  'list '+  (send  (eval  point) :  list-format) 
'(0  0  10))) 

point)))) 


(defun  subs-point-info-equation  (Plane  Point) 
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(apply  ’+  (map  ’list  ’*  (send  (eval  Point)  :Hst-fonnat)  Plane))) 


(defun  subs-line-into-plane-equatlon  (Line  Piane)  ;  TRUE  M  lines  iie  in  plane 
(let*  ((endpoints  (send  (eval  Line)  :endpoints)) 

(point-Aos  (list  (send  (eval  plane) 

:subs-point*into-plane  (first  endpoints)) 

(send  (eval  plane) 

:subs-point-into-plane  (second  endpoints))))) 

(apply  'and 

(map  ’list  #’equal-error 
poInt-Aos 

(make-list  2  :initial-element 

(fourth  (send  (eval  piane)  :list-coeff))))))) 

; - MANIPULATE  FACETS - 


(defun  find-common-facet  (VI  V2)  ;  find  the  first  facet  that  two  volumes  have  in 
;  common.  Use  the  assumption  that  common  facets 
;  will  have  same  name  first,  else  they  will  have 
;  the  same  plane  equation. 

(let  ((common-facet  (first  (intersection  (volume-facets  (eval  VI)) 

(volume-facets  (eval  V2)))))) 

(cond  ((not  (null  common-facet)) 

(retum-from  find-common-facet  common-facet)) 

((not  (null  (facet-connects  (eval  (first  (volume-facets  (eval  VI ))))))) 

(loop  for  n  in  (volume-facets  (eval  VI)) 
do  (cond  ((member-p  V2  (second  (facet-connects  (eval  FI)))) 
(retum-from  find-common-facet  FI))))) 

(t  (loop  for  FI  in  (volume-facets  (eval  VI)) 

do  (loop  for  F2  in  (volume-facets  (eval  V2)) 

do  (cond  ((send  (eval  FI)  :test-equal  F2) 

(return-from  find-common-facet  F2)))))))) 


’nil) 


(defun  mean-point-ln-facet  (Facet) 

(map  ’list  ’(lambda  (a  b)  (/  a  b))  (mean-point-in-facet-2  (send  (eval  Facet)  .points)) 
(make-list  3  :initial-element 

(length  (send  (eval  Facet)  :points))))) 


(defun  mean-point-in-facet-2  (Points) 

(cond  ((null  Points)  ’(0  0  0)) 

(t  (map  ’list  ’+  (send  (eval  (first  Points))  ;llst-format) 
(mean-point-in-facet-2  (rest  Points)))))) 


(defun  info-on-facets  (list-of-facets)  ;  find  all  points  and  lines  in  a  list  of  facets 
(let  ((lines  ’nil) 

(points  ’nil)) 
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(loop  for  F  In  llst-of-facets 
do  (let  0 

(setf  lines  (append  (facet-edges  (eval  F))  lines)) 

(setf  points  (append  (send  (eval  F)  :points)  points)))) 
(setf  lines  (remove-duplicates  lines)) 

(setf  lines  (remove  ’nil  lines)) 

(setf  points  (remove-duplicates  points)) 

(setf  points  (remove  ’nil  points)) 

(list  points  lines))) 


(defun  inside-facet-p  (point  facet)  ;  return  T  iff  point  is  inside 

(let  ((horizontal-plane  (make-z-plane  point))  ;  a  convex  facet 
(vertical-y-plane  (make-y-plane  point)) 

(vertical-x-plane  (make-x-plane  point)) 

(vertical-Ao-x  ’nil) 

(vertical-Ao-y  ’nil) 

(left-points  ’nil) 

(right-points  ’nil) 

(edge-points  ’nil)) 

;  intercept  all  edges  with  horizontal  plane, 

;  plane  interception  points  in  left  or  right 
;  half,  based  upon  relationship  with  vertical  plane 

(setf  vertical-Ao-x  (fourth  (send  (eval  vertical-x-plane)  ;list-coeff))) 

(setf  vertical-Ao-y  (fourth  (send  (eval  vertical-y-piane)  :list-coeff))) 

(loop  for  L  in  (facet-edges  (eval  Facet)) 
do  (let  ((I  (find-intercept-point  horizontal-plane  L)) 

(l-Ao-x  ’nil) 

(l-Ao-y  ’nil)) 

(cond  ((not  (equal  'nil  I)) 

(setf  l-Ao-y  (send  (eval  vertical-y-plane)  :subs-point-into-plane  I)) 
(setf  l-Ao-x  (send  (eval  vertical-x-plane)  :subs-point-into-plane  I)) 
(cond  ((LT  vertical-Ao-x  l-Ao-x) 

(setf  right-points  (adjoin  I  right-points))) 

((GT  vertical-Ao-x  l-Ao-x) 

(setf  left-points  (adjoin  I  left-points))) 

(t  (sett  edge-points  (adjoin  I  edge-points)))) 

(cond  ((LT  vertical-Ao-y  l-Ao-y) 

(setf  right-points  (adjoin  I  right-points))) 

((QT  vertical-Ao-y  l-Ao-y) 

(setf  left-points  (adjoin  I  left-points))) 

(t  (setf  edge-points  (adjoin  I  e^ge-points)))))))) 

;  test  for  inclusion  by  nr  of  intercept  points 

(cond  ((or  (not  (evenp  (length  left-points)))  ;  if  either  one  odd,  then  point 
(not  (evenp  (length  right-points))))  ;  is  in  facet 
(retum-from  inside-facet-p ’t)) 

(t  (return-from  inside-facet-p  ’nil))))) 
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MAKE  OR  MANIPULATE  VOLUMES- 


(defun  intersect-all-planes-with-volumes  (list-of-planes  LIst-of-volumes) 

;  intersectal  all  planes  given  with  all  volumes  given, 

;  Including  resultant  volumes  from  earlier  intersections. 

;  requires  input  of  volumes  as;  ((volume)  (volume) ...) 

;  resultant  volume  list  is  the  same  format. 

(let  ((old-llst-of-error-planes  ’nil) 

(result-volumes 

(intersect-all-planes-with-volumes-2Lisl-of-planes  Ust-of-volumes))) 
(loop  repeat  ’1 
do  (let  0 

(terpii)  (terpri) 

(princ "  Re-doing  error  intercepts:  *) 

(print  *list-of-error-planes*)  (terpri) 

(set!  old-list-of-enror-planes  *list-of-error-planes*) 

(set!  *list-of-error-planes*  ’nil) 

(setf  result-volunres  (intersect-all-planes-with-volumes-2 

old-Hst-of-error-planes 
result- volumes)))) 

result-volumes)) 


(defun  intersect-all-planes-with-volumes-2  (List-of-planes  List-of-volumes) 

;  do  all  the  work  for  intersect-all-planes-with-volumes 

(let  ((templist  ’())) 

(cond  ((null  list-of-planes)  list-of-volumes) 

(t  (loop  for  V  in  Ust-of-volumes 
do  (let  ((temp  ’nil)) 

(setf  temp  (intersect  (car  V) 

(send  (eval  (car  list-of-planes)) 
:list-coeff))) 

(cond  ((equal  ’1  (length  temp)) 

(push  temp  templist)) 

(t  (push  (list  (first  temp))  templist) 

(push  (list  (second  temp))  templist))))) 
(intersect-all-planes-wlth-volumes-2  (cdr  llst-of-planes)  templist))))) 


; - PROPERTY  LIST  MANIPULATIONS 

(defun  reset-point-property-llsts  (Volume) 
(loop  for  P  in  (volume-points  (eval  Volume)) 
do  (setf  (get  P  ’lines)  ’nil) 
do  (setf  (get  P  ’planes)  ’nil) 
do  (setf  (get  P  ’distance)  ’nil))) 
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; - MANIPULATE  GLOBAL  COUNTERS - 

(defun  speed-demon  () 

(terpii)  (terpri)  ;  delete  *list-of-?????‘  lists  to 

(princ  "*******SPEED-DEMON-INVOKED*******")  ;  speed  processing,  best  if 
(terpri)  (terpri)  ;  used  with  static  universe  methods 

(sett  ‘iist-of-vectors*  ’nii)  ;  if  contents  of  old  lists  still  needed 

(setf  *llst-oMines*  ’nil) 

(setf  *list-of-polnts*  ’nil) 

(setf  *list-of-planes*  ’nil) 

(make-null-vector) 

(make-origin)) 


; - more  specific  stuff - 

(defun  minimum-distance  (lines  start-point) 

(let  ((best-line  (first  lines))) 

(cond  ((<  '1  (length  lines)) 

(loop  for  L  in  (rest  lines) 

do  (cond  ((>  (get  (send  (eval  L)  .other-end  start-point) 
’distance) 

(get  (send  (eval  best-line)  ;other-end  start-point) 
’distance)) 

(setf  best-line  L)))))) 

best-iine)) 


FIND  THE  VOLUME(S)  CONTAINING  A  GIVEN  POINT 


(defun  locate-point  (point) 

;  return  the  one,  two,  or  more  volumes  which  contain  the  point. 
;  muitipie  volumes  are  possible  if  point  is  on  facet  or  vertex 
;  of  a  volume 

(let  ((list-of-possible-volumes  (universe-volumes  'universe*)) 
(reject-volumes  ’nil) 

(x-plane  (make-x-plane  point)) 

(y-plane  (make-y-plane  point)) 

(z-plane  (make-z-plane  point))) 

;  loop  through  planes  which  define  point, 

;  removing  volumes  which  do  not  intersect  the  planes. 

;  point  is  located  in  volume(s)  which  are  left 

(loop  for  PI  in  (iist  x-piane  y-plane  z-plane) 
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do  (let  0 


;  loop  through  (modified)  list  of  candiate  voiumes 
(loop  for  V  in  list-of-possible-volumes 

do  (let  ((first-point-Ao  (send  (eval  PI)  :subs-point-into-plane  < 

(first  (volume-points  (eval  V))))) 

(Ao  (fourth  (send  (eval  PI)  :list-coeff)))) 

;  see  if  volume  stratUes  plane 

(cond  ((not  (equal-error  first-point-Ao  Ao)) 

(cond  ((point-check-p  PI  first-point-Ao  Ao  V) 

(setf  reject-volumes 

(adjoin  V  reject-volumes)))))))) 

;  remove  rejected  volumes  from  possible  location  of  points 

(loop  for  V  in  reject-volumes 

do  (setf  list-of-possible-volumes  (remove  V  list-of-possible-volumes))) 

(setf  reject- volumes  ’nil))) 

;  select  actual  location  of  point  from  final  list 
;  of  volumes 

(loop  for  V  in  list-of-possible-volumes  ;  not  so  good 
do  (let  ((lines  ’nil)) 

(loop  for  F  in  (volume-facets  (eval  V)) 
do  (setf  (get  F  ’center)  (init-point  (mean-point-ln-facet  F))) 
do  (setf  lines  (adjoin  (make-line  Point  (get  F  ’center))  lines))) 

(cond  ((lines-strattle-facets-p  Lines  V) 

(setf  list-of-possible-volumes  (remove  V  list-of-possible-volumes)))))) 

list-of-possible-volumes)) 

(defun  locate-point-air  (point) 

;  return  the  one,  two,  or  more  air  volumes  which  contain  the  point. 

;  multiple  volumes  are  possible  if  point  is  on  facet  or  vertex 
;  of  a  volume.  Same  as  locate-point  function,  except  that  ground 
;  volumes  are  removed 

(let  ((list-of-possible-volumes  (universe-volumes  'universe*)) 

(reject-volumes  ’nil) 

(x-plane  (make-x-plane  point)) 

(y-plane  (make-y-plane  point)) 

(z-plane  (make-z-plane  point))) 

;  loop  through  planes  which  define  point, 

;  removing  volumes  which  do  not  Intersect  the  planes. 
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;  point  is  located  in  volunne(s)  which  are  left 

(loop  for  PI  in  (list  x*plane  y-plane  z-plane) 
do  (let  0 


;  loop  through  (nradified)  list  of  candiate  volumes 

(loop  for  V  in  list-of-possible-volumes 
do  (let  ((first-poInt-Ao  (send  (eval  PI)  :subs-point-into-plane 

(first  (volume-points  (eval  V))))) 

(Ao  (fourth  (send  (eval  PI)  ;llst-coeff)))) 

;  see  if  volume  stratties  plane 

(cond  ((not  (equal-error  fIrst-point-Ao  Ao)) 

(cond  ((point-check-p  PI  first-point-Ao  Ao  V) 

(setf  reject-volumes 

(adjoin  V  reject-volumes)))))))) 

;  remove  rejected  volumes  from  possible  location  of  points 

(loop  for  V  in  reject-volumes 

do  (set!  list-of-possible-volumes  (remove  V  list-of-possible-volumes))) 

(setf  reject-volumes  ’nil))) 

;  select  actual  location  of  point  from  final  list 
;  of  volumes 

(loop  for  V  in  list-of-possible-volumes  ;  not  so  good 
do  (let  ((lines  ’nil)) 

(loop  for  F  in  (volume-facets  (eval  V)) 
do  (setf  (get  F  ’center)  (init-point  (mean-point-in-facet  F))) 
do  (setf  lines  (adjoin  (make-line  Point  (get  F  ’center))  lines))) 

(cond  ((lines-strattle-facets-p  Lines  V) 

(setf  list-of-possible-volumes  (remove  V  list-of-possible-volumes)))))) 


;  remove  ground  volumes  from  list 

(loop  for  V  in  list-of-possible-volumes 
do  (cond  ((equal  ’ground  (volume-composition  (eval  V))) 

(setf  list-of-possible-volumes  (renfwve  V  llst-of-possible-volumes))))) 

list-of-possible-volumes)) 


(defun  point-in-volume-p  (point  volume) ;  return  T  Iff  the  point  Is  Inside  the  volume 

;  return  NIL  otherwise 

;  code  is  modified  version  of  locate-point-air 
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(let  ((Ilst-of-possiWe-volumes  (list  volume)) 

(reject-volumes  ’nil) 

(x-plane  (make-x-plane  point)) 

(y-plane  (make-y-plane  point)) 

(z-plane  (make-z-plane  point))) 

;  see  if  point  is  a  vertex,  or  in  a  facet  of  the  volume 

(cond  ((member-p  point  (volume-points  (eval  volume))) 
(retum-from  point-in-volume-p ’t))) 

(loop  for  F  in  (volume-facets  (eval  volume)) 
do  (cond  ((Inside-facet-p  point  F) 

(return-from  poInt-in-volume-p ’t)))) 

;  loop  through  planes  which  define  point, 

;  removing  volumes  which  do  not  intersect  the  planes. 
;  point  is  located  in  volume(s)  which  are  left 

(loop  for  PI  in  (list  x-plane  y-plane  z-plane) 
do  (let  0 


;  loop  through  (modified)  list  of  candiate  volumes 

(loop  for  V  in  list-of-possible-volumes 
do  (let  ((first-point-Ao  (send  (eval  PI)  :subs-point-into-plane 

(first  (volume-points  (eval  V))))) 

(Ao  (fourth  (send  (eval  PI)  :list-coeff)))) 

;  see  if  volume  strattles  plane 

(cond  ((not  (equal-error  first-point-Ao  Ao)) 

(cond  ((point-check-p  PI  first-point-Ao  Ao  V) 

(setf  reject-volumes 

(adjoin  V  reject-volumes)))))))) 

;  remove  rejected  volumes  from  possible  location  of  points 

(loop  for  V  in  reject-volumes 

do  (setf  list-of-possible-volumes  (remove  V  list-of-possible-volumes))) 
(setf  reject-volumes  ’nil))) 

(cond  ((null  list-of-possible-volumes)  ;  exit  condition 

(retum-from  pnint-in-volume-p  ’nil))) 


’0) 

(oefun  point-check-p  (Plane  Basis-point-Ao  Ao  Volume) 
(loop  for  P  in  (rest  (volume-points  (eval  Volume))) 
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do  (let  ( (next-point- Ao  (send  (eval  Plane)  :subs-point-into-plane  P))) 
(cond  ((equal  next-point-Ao  Ao) 

(retum-from  point-check-p  ’nil)) 

((or  (and  (GT  Ao  Next-point-Ao) 

(LT  Ao  basis-point-Ao)) 

(and  (LT  Ao  Next-point-Ao) 

(QT  Ao  basis-point-Ao))) 

(retum-from  poInt-check-p  ’nil))))) 

■t) 


(defun  iines-strattle-facets-p  (Lines  Volume) 

(loop  for  L  in  Lines 

do  (loop  for  F  In  (volume-facets  (eval  Volume)) 

do  (cond  ((send  (eval  L)  ;strattle-plane-p  F) 

(retum-from  lines-strattle-facets-p ’t))))) 
’nil) 


PRINT  GOOD-TO-KNOW  INFO  CONCERNING  THE  STATE 
OF  THE  ‘UNIVERSE*  INTO  A  DISK  FILE 


(defun  dump-volumes  (list-of-volumes) 

(setq  ‘output-stream*  (open  ’'exp3:lewis;run2’’  :direction  :output)) 

(print  "sending  data  to  file  (run2)...") 

(loop  for  V  in  List-of-volumes 
do  (let  0 

(terpri  ‘output-stream*)  (terpri  ‘output-stream*)  (terpri  ‘output-stream*) 
(print-volumes  (list  V)) 

(terpri  ‘output-stream*) 

(print-points  (volume-points  (eval  V))) 

(terpri  ‘output-stream*) 

(print-lines  (volume-edges  (eval  V))) 

(terpri  ‘output-stream*) 

(print-facets  (volume-facets  (eval  V))))) 

(terpri  ‘output-stream*)  (terpri  ‘output-stream*)  (terpri  ‘output-stream* ) 

(close  'output-stream') 

(print  "Done.")  ’nil) 


(defun  dump-path  (path-name) 

(setq  'output-stream'  (open  "exp3:lewis:path-dump*  :direction  :output)) 
(print  "sending  path  data  to  file  (path-dump)...") 

(terpri  'output-stream')  (terpri  'output-stream')  (terpri  'output-stream') 
(print-path  path-name) 

(terpri  ‘output-stream*) 

(print-points  (path-points  (eval  path-name))) 
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(terpri  'output-streanri*) 

(piint-lines  (path-lines  (eval  path-name))) 

(terpri  'output-stream*) 

(print-facets  (path-facets  (eval  path-name))) 

(terpri  'output-stream*)  (terpri  'output-stream')  (terpri  'output-stream') 
(close  'output-stream') 

(print  "Done.") 

’nil) 


PRINT  FLAVOR  FUNCTIONS 


20  May  191 


(defun  print-points  (List) 

(cond  ((null  List)) 

(t  (terpri  'output-stream') 

(print  'name: "  'output-stream') 
(print  (car  List)  'output-stream') 
(send  (eval  (car  List))  :print) 
(print-points  (cdr  List))))) 

(defun  print-vectors  (List) 

(cond  ((null  List)) 

(t  (terpri  'output-stream') 

(print  "name: '  'output-stream') 
(print  (car  List)  'output-stream') 
(send  (eval  (car  List))  :print) 
(print-vectors  (cdr  List))))) 

(defun  print-lines  (List) 

(cond  ((null  List)) 

(t  (terpri  'output-stream') 

(print  "name:'  'output-stream') 
(print  (car  List)  'output-stream') 
(send  (eval  (car  List))  :print) 
(print-lines  (cdr  List))))) 

(defun  print-facets  (List) 

(cond  ((null  List)) 

(t  (terpri  'output-stream') 

(print  "name:"  'output-stream') 
(print  (car  List)  'output-stream') 
(send  (eval  (car  List))  :print) 
(print-facets  (cdr  List))))) 
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(defun  print-volumes  (List) 

(cond  ((null  List)) 

(t  (terpri  *output-stream*) 

(print  "name:" ‘output-stream*) 

(print  (car  List)  ‘output-stream*) 

(send  (eval  (car  List))  :print) 

(pdnt-volunrres  (cdr  List))))) 

(defun  print-path  (name) 

(terpri  ‘output-stream*) 

(princ  "name: "  ‘output-stream*)  (print  name  ‘output-stream*) 

(princ  ’start-point: "  ‘output-stream*) 

(print  (path-start-point  (eval  name))  ‘output-stream*) 

(terpri  ‘output-stream*) 

(princ  "end-point:  ’‘output-stream*) 

(print  (path-end-point  (eval  name))  ‘output-stream*) 

(terpri  ‘output-stream*) 

(princ  ‘volumes:  "output-stream*) 

(print  (path-volumes  (eval  name))  ‘output-stream*) 

(terpri  ‘output-stream*) 

(princ  ’facets:  "output-stream*) 

(print  (path-facets  (eval  name))  ‘output-stream*) 

(terpri  ‘output-stream*) 

(princ  "lines:  "output-stream*) 

(print  (path-lines  (eval  name))  ‘output-stream*) 

(terpri  ‘output-stream*) 

(princ  "points:  "*output-stream*) 

(print  (path-points  (eval  name))  "output-stream") 

(terpri  "output-stream") 

(princ  "length:  "‘output-stream*) 

(print  (path-length  (eval  name))  *output-stream*) 

(terpri  *output-stream*) 

(princ  "total  K  values:  "‘output-stream*) 

(print  (path-total-K  (eval  name))  "output-stream") 

(terpri  "output-stream") 

(princ  "maximum  detection  probability:  ""output-stream") 

(print  (path-max-detection-probability  (eval  name))  "output-stream*) 
(terpri  "output-stream") 

(princ  "average  detection  probability;  ""output-stream") 

(print  (path-ave-detection-probability  (eval  nanw))  "output-stream") 
(terpri  *output-stream*)) 
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FILE  NAME:  Interception . lisp 
ModerUsp;  Syntax:  Common-lisp 

;  FUNCTIONS  TO  INTERCEPT  A  VOLUME  WITH  A  PLANE  D.H.LEWIS  27May88 


These  functions  intercept  planes  wtth  volumes  arnf  lines  with  planes.  Multiple 
tests  are  performed  to  ensure  proper  construction  of  new  volumes.  Facets  are 
rebuilt  each  time. 

Main  functions:  iNTERSECT  (VOLUME  PLANE) 

FIND-iNTERCEPT-POINT  (PLANE  LINE) 

Other  functions:  GET-INTERCEPT-POINT  (PLANE  LINE  T-INTERCEPT) 
PUT-LINE-IN-CORRECT-HALF  (LINE  PLANE) 

PUSH-ENDPOINTS  (LINE  VOLUME) 

MAKE-NEW-DIVIDING-LINES  (VOLUME  OLDPOINTS  NEW-POINTS) 
NEW-VALID-LINE  (POINT1  POINT2  VOLUME) 

IN-FACET-P  (POINT1  POINT2  FACET) 

OUTSIDE-VOLUME  (LINE  VOLUME) 

DENOM-IN-INTERCEPT  (PLANE  LINE) 

GET-INTERCEPT-POINT-2  (LINE  T-INTERCEPT) 


(defvar  *lines-in-intercept-plane*  ’nil) 

(defvar  ‘large-integer*  ’1e4) 

(defvar  *list-of-error-planes*  ’nil)  ;  used  to  correct  errors  in  interceptions 

(defun  intersect  (Volume  Plane) 

(let  ((old-precision  ‘precision*) 

(bad-euler-flag ’t) 

(new-volumel  ’nil) 

(new-volume2  ’nil) 

(facet-planes  ’nil) 

(intercept-plane  ’nil)) 

(terpri)  (princ  "intersecting: ")  (print  (list  Volume  Plane)) 

(princ "  —  Result: ") 

(sett  ‘lines-in-intercept-plane*  'nil) 

(cond  ((bad-intersect-preconditions-p  Volume  Plane);  check  for  degenerate  conditions 
(retum-from  intersect  (list  volume)))) 

(setf  intercept-plane  (init-plane  Plane)) 

(loop  for  F  in  (volume-facets  (eval  Volume))  ;  get  all  planes  used 
do  (setf  facet-planes  (adjoin  (init-plane  (send  (eval  F)  :list-coeff)) 

facet-planes))) 

(setf  facet-planes  (adjoin  intercept-plane  facet-planes)) 

(setf  facet-planes  (remove-duplicates  facet-planes)) 

(loop  until  (or  (not  bad-euler-flag)  (>  ‘precision*  (‘  '5  old-precision))) 
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do  (let  0 

;  dear  standard  volunnes  before  use  (or  reuse) 

;  and  set  common  values 

(send  *above*  rdear) 

'  (setf  (volume-visibility  ’above*)  (volume-visibility  (eval  Volume))) 

(setf  (voiume-oomposition  *above*)  (volume-composition  (eval  Volume))) 

(send  'below*  rdear) 

(setf  (volume-visibility  'below*)  (volume-visibility  (eval  Volunte))) 

(setf  (volume-composition  'below')  (volume-composition  (eval  Volume))) 

;  conduct  intercept 

(let  ((List-of-new-points  ’nil) 

(list-of-old-points  ’nil)) 

(loop  for  P  In  (volume-points  (eval  Volume)) 
do  (setf  (get  P  ’lines)  ’nil)) 

;intersed  each  line  of  volume 
(loop  for  Line  in  (Volume-Edges  (evd  Volume)) 
do  (let  ((new-point  (find-intercept-point  intercept-plane  Line))) 

(cond  ((equal  new-point  ’nil) 

(cond  ((not  (subs-line-into-plane-equation  Line 

intercept-plane)) 

(put-line-in-correct-half 

Lir>e 

(first  (send  (eval  Line)  rendpoints)) 
intercept-plane)))) 

•  ((member-p  new-point  (Volume-points  (eval  Volume))) 

(pushnew  new-point  list-of-old-points) 
(put-line-in-corred-half 

*  Line  new-point  intercept-plane)) 

(t  (pushnew  new-point  List-of-new-points) 

(place-intercept-point  Plane  Line  New-point))))) 

(make-new-dividing-lines  Volume  LIst-of-new-points  Ilst-of-okJ-poInts)) 

(cond  ((not  (simple-volume-test-p))  ;  check  degenerate  cases 
(setf  'predsion'  oid-predslon) 

(return-from  intersect  (list  volume)))) 

;  build  new  facets  in  best  way  possible 

(cond  ((not  *not-convex-volumes*)  ;  do  convex  facets 

(make-facets  facet-planes  'above')  ;  quick  facet  builder 
(make-facets  facet-planes  'below') 

(cond  ((not  (check-eulers-relation-p)) 

(setf  (volume-facets  'above')  ’nil) 

(setf  (volume-facets  'below')  ’nil) 

(make-all-facets  'above')  ;  slow  facet  builder 
(make-all-facets  'below')))) 

(t  (make-ail-facets  'above')  ;  do  non-convex  facets 
(make-all-facets  'below'))) 
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(cond  ((null  (Intersection  (volume-facets  *above*) 

(volume-facets  ’below*))) 

(force-facet  plane))) 

(cond  ((not  (check-eulers-relatlon-p)) 

(setf  *predsion*  (*  *predsion*  ’2.0))) 

(t  (setf  bad-euler-flag  ’nil))))) 

(cond  ((not  bad-euler-flag) 

(setf  new-volumel  (make-volume-name))  ;  give  legitimate  names  to  new 
(setf  new-volume2  (make-voiume-name))  ;  volumes 
(send  ’above*  :make-equal  new-volumel) 

(send  ’below’  :make-equal  new-voiume2) 

(push  new-volumel  ’list-of-volumes’) 

(push  new-volume2  ’llst-of-volumes’) 

(setf  ’precision*  old-precision) 

(prini  (intersection  (volume-facets  (eval  new-volumef )) 

(volume-facets  (eval  new-volume2)))) 

(retum-from  intersect  (iist  rtew-volumel  new-voiume2)))  ;  return  new  voiumes 
(t  (setf  ’precision*  old-precision) 

(setf  *list-of-error-planes’ 

(adjoin  intercept-plane  *list-of-error-planes’)) 

(retum-from  intersect  (list  Volume)))))) 

(defun  subs-point-into-plane  (Pt  Plane) 

(send  (eval  Plane)  :subs-polnt-into-plane  Pt)) 

(defun  bad-intersect-preconditions-p  (Volume  Plane)  ;  test  for  null  plane  (0  0  0  0) 

;  and  facet  Intercept  if  convex 

(cond  ((equal  ’zero-vector*  (map  'list  '*  plane  *one-vector*)) 

(princ  "nil  (early  1)") 

(retum-from  bad-intersect-preconditions-p ’t)) 

:  ((not  *not-convex-volumes*) 

:  (loop  for  F  in  (volume-facets  (eval  Volume)) 

;  do  (cond  ((send  (eval  F)  :test-equat  (Init-plane  Plane)) 

;  (princ  "nil  (early  2)") 

;  (retum-from  bad-intersect-preconditions-p ’t) ) ) ) ) 

) 

’nil) 

(defun  find-intercept-point  (plane  line) ;  find  intercept  point  of  plane  and  line 

;  segment,  if  it  exists,  return  NIL 
;  If  not  exist 

(let  ((denom  (rationalize  (denom-in-intercept  plane  line))) 

(t-lntercept  ’nil) 

(l-point  ’nil)) 

(cond  ((not  (equal-zero-p  denom)) 

(setf  t-intercept  (rationalize  (solve-for-t 

(send  (eval  plane)  :list-coeff) 
line 
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denom))) 

(setf  l-point  (get-intercept-point-2  line  t-intercept)))) 

l-point)) 

(defun  denom-in-intercept  (plane  line) ;  find  the  denominator  in  intercept  equation 
(apply  ’+  (map  list  ’*  (send  (eval  plane)  :list-coeff) 

(map  list  ’rationalize 

(send  (eval  (line-segment-direction-vector 
(eval  line)))  :list-fomiat))))) 

(defun  get-intercept-point-2  (line  t-intercept) 

;  return  the  name  of  a  valid  intercept  point 

(let  ((I  ’nil) 

(l-llst  ’nil)) 

(cond  ((not  (or  (GT  t-intercept  (line-segment-t-max  (eval  line))) 

(LT  t-intercept  ’0.0))) 

(setf  l-list  (vector-add-with-t 

(line-segment-direction-vector  (eval  line)) 
(line-segment-position-vector  (eval  line)) 
t-intercept)) 

(setf  I  (init-point  l-list)))) 

I)) 

(defun  place-intercept-point  (Plane  Line  I) ;  divide  old  line  at  I,  build  new  lines 
(let  ((L1  ’nil)  ;  and  put  each  in  right  resultant  volume 

(L2  ’nil)) 

(setf  (get  I  ’lines)  Line) 

(pushnew  I  (volume-points  'above*)) 

(pushnew  I  (volume-points  'below')) 

(setf  LI  (make-line  I  (first  (send  (eval  Line)  :endpolnts)))) 

(setf  L2  (make-line  I  (second  (send  (eval  Line)  :endpoints)))) 

(setf  (line-segment-characteristics  (eval  LI ))  ;  ridge  is  still  a  ridge 

(line-segment-characteristics  (eval  Line))) 

(setf  (line-segment-characteristics  (eval  L2)) 

(line-segment-characteristics  (eval  Line))) 

(cond  ((LT  (fourth  Plane)  ;  which  volume  to  put  new  line  L1  ? 

(subs-point-into-equation  Plane  (car  (send  (eval  Line)  lendpoints 

)))) 

(pushnew  LI  (volume-edges  'above')) 

(push-endpoints  LI  ’'above')) 

((QT  (fourth  Plane) 

(subs-point-into-equation  Plane  (car  (send  (eval  Line)  :endpoints 

)))) 

(pushnew  LI  (volume-edges  'below')) 

(push-endpoints  LI  ’'below')) 

(t)) 

(cond  ((LT  (fourth  Plane)  ;  Which  volume  to  put  new  line  L2? 

(subs-point-into-equation  Plane  (cadr  (send  (eval  Line)  :endpoints 

)))) 

(pushnew  L2  (volume-edges  'above')) 

(push-endpoints  L2  ’'above')) 
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((QT  (fourth  Plane) 

(subs-point-Into-equation  Plane  (cadr  (send  (eval  Line)  :endpoints 

)))) 

(pushnew  L2  (volunre-edges  *below*)) 

(push-erKipoints  L2  '*below*))))) 

(defun  put-line-in-correct-half  (Line  Poirtt  Plane) ;  put  a  preexisting  volume  line 

;  into  the  correct  resultant  volume 
(let  ((Plane-Ao  (fourth  (send  (eval  Plane)  .llst-coeff))) 

(other-point  (send  (eval  Line)  ;other-end  Point))) 

(cond  ((GT  (send  (eval  Plane)  rsubs-polnt-into'ptane  other-point) 

Plane-Ao) 

(pushnew  Line  (volume-edges  *£^ove*)) 

(push-endpoints  Line  ’*above*)) 

(t  (pushnew  Line  (volume-edges  *below*)) 

(push-endpoints  Line  '*below*))))) 


(defun  push-endpoints  (Line  Volume) 

(pushnew  (first  (send  (eval  Line)  ;endpoints))  (volunrte-points  (eval  Volume))) 
(pushnew  (second  (send  (eval  Line)  :endpolnts))  (volume-points  (eval  Volume)))) 


(defun  make-new-dividing-lines  (Volume  List-new-points  Ust-old-points) 

(loop  for  PI  in  List-new-points  ;  handie  case  where  no  pre-exitant  points  invoived 
do  (loop  for  P2  in  List-new-points 
do  (cond  ((not  (equal  PI  P2)) 

(new-valid-line  P1  P2  Volume))))) 

(loop  for  P1  in  Ust-old-points  ;  add  pre-exItant  lines  and  points 
do  (loop  for  P2  in  List-old-points  ;to  new  volumes 
do  (cond  ((not  (equal  PI  P2)) 

(new-valid-line  PI  P2  Volume)  ;make  new  connecting  lines 
;then  find  old  ones 

(loop  for  Une  in  (volume-edges  (eval  Volume)) 

do  (let  ((endpointi  (first  (send  (eval  Line)  -.endpoints))) 

(endpoint2  (second  (send  (eval  Une)  :endpoints)))) 
(cond  ((and  (or  (equal  PI  endpointi) 

(equal  PI  endpoint2)) 

(or  (equal  P2  endpointi ) 

(equal  P2  endpolnt2))) 

(push-endpoInts  Line  ’above*) 

(push-endpoInts  Line  ’below*) 

(pushnew  Line  (volume-edges  ’above*)) 
(pushnew  Line  (volume-edges  ’below’)) 
(pushnew  Line  ’Hnes-in-intercept-plane’))))))))) 

(loop  for  P-new  in  List-new-points  ;  add  new  lines  connecting  old  and  rtew 
do  (loop  for  P-old  in  Ust-old-points  ;  points  to  new  volumes 
do  (new-valid-line  P-new  P-old  Volume)))) 
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(defun  new-valW-llne  (P1  P2  Volume) ;  make  a  new  (and  valid)  line  between  Pi  and  P2 
;  which  lies  inside  (or  along  an  edge)  of  Volume 
(loop  for  FI  In  (volume-facets  (eval  Volume)) 
do  (cond  ((in-facet-p  PI  P2  FI) 

(let  ((Line  (make-line  PI  P2))) 

(cond  ((not  (outsWe-volume  Line  Volume)) 

(push-endpoints  Line  *above*) 

(push-endpoints  Line  *below*) 

(pushnew  Line  (volume-edges  *above*)) 

(pushnew  Line  (volume-edges  *below*)) 

(pushnew  Line  *Hrtes-in-lntercept-plane*)))))))) 


(defun  simple-volume-test-p  () 

(cond  ((or  (or  (>  '3  (length  (volume-points  ‘above*))) 

(>  ’3  (length  (volume-points  ‘below*)))) 

(or  (>  '5  (length  (volume-edges  ‘above*))) 

(>  '5  (length  (volume-edges  ‘below*))))) 

(princ  "nil  (late  1)") 

(retum-from  slmple-volume-test-p  ’nil))) 

’t) 

(defun  check-eulers-relation-p  () 

(cond  ((or  (not  (equal  ’2  (+  (length  (volume-pointe  ‘above*)) 

(length  (volume-facets  ‘above*)) 

(-  '0  (length  (volume-edges  ‘above*)))))) 

(not  (equal  '2  (+  (length  (volume-points  ‘below*)) 

(length  (volume-facets  ‘below*)) 

(-  ’0  (length  (volume-edges  ‘below*))))))) 
(princ "  Violated  Eulers  relation ")  (print  ‘precision*) 

(terpri) 

(princ '  ") 

(return-from  check-euiers-reiation-p  ’nil))) 

’t) 

(defun  make-facets  (planes  volume) 

(loop  for  P  in  planes  ;  dear  plane  properties 

do  (setf  (get  P  ’edges)  ’nil)) 

(loop  for  P  in  planes  ;  find  which  lines  lie  in  which  planes 

do  (loop  for  E  in  (volume-edges  (eval  Volume)) 

do  (cond  ((subs-line-into-plane-equation  E  P) 

(setf  (get  P  ’edges)  (adjoin  E  (get  P  ’edges))))))) 

(ioop  for  P  in  pianes  ;  buiid  iegitimate  facets 

do  (cond  ((and  (not  (null  (get  P  ’edges))) 

(<-  ’3  (length  (get  P  ’edges)))) 

(setf  (volume-facets  (eval  Volume)) 

(adjoin  (init-facet-2  (list  (get  P  'edges)  P)) 
(volume-facets  (eval  Volume))))))) 
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(loop  for  P  in  planes 
do  (sett  (get  P  ’edges)  ’nil))) 


;  dear  plane  properties 


(defun  force-facet  (Plane)  ;  force  a  facet  to  exist,  if  all  else  fails 

(let*  ((lines-in-facet  *lines-in-intercept-plane*)  ( 

(forced-facet  (init-facet-2  (list  lirtes-in-facet  (InK-plane  Plane))))) 

(setf  (volume-facets  *above*)  (adjoin  forced-facet  (volume-feK^ets  'above*))) 

(setf  (volume-facets  *befow*)  (adjoin  forced-facet  (volume-facets  *below*))) 

(princ '  Forced  *)))  * 


(defun  in-facet-p  (P1  P2  F)  ;  return  T  iff  points  PI  and  P2  are  inside  facet  F 
(cond  ((and  (or  (member-p  (get  PI  ’lines)  (facet-edges  (eval  F))) 

(member-p  PI  (send  (eval  F)  :polnts))) 

(or  (member-p  (get  P2  ’lines)  (facet-edges  (eval  F))) 

(member-p  P2  (send  (eval  F)  rpoints)))) 

(retum-from  In-facet-p ’t)) 

(t  (retum-from  in-facet-p  ’nil)))) 


(defun  outside-volume  (Line  Volume)  ;  return  T  iff  line  is  outside  the  volume 
;  do  only  if  dealing  with  ground  volumes  or 
;  non-convex  air  volumes 
(cond  ((or  *not-convex-volumes* 

(equal  'ground  (volume-composition  (eval  volume)))) 

(let  ((mid-point  (init-point  (send  (eval  line)  :midpoint)))) 

(cond  ((point-in-volume-p  mid-point  vc^ume) 

(return-from  outside-volume  ’nil)) 

(t  (retum-from  outside-volume ’t))))) 

(t  (retum-from  outside-volume  ’nil)))) 
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FILE  NAME:  Ceunera.lisp 


;;  Mode:  LISP;  Syntax:  Common-lisp 


;;  FLAVORS  FOR  3-D  DISPLAY  OF  VOLUMES 


;for  CS4452 

;;  THESIS  D.H.  Lewis 


;Written  by  Dr  Sehung  Kwak 
18  May  1988 


(defflavor  Graphic 
(node-list 
polygon-list 
transformed-node-list 
H-matrix) 

0 

:inittable-instance-variables 
:settable-instance- variables 
:gettable-instance-variables 
:outside-accessible-instance-variables) 

(defmethod  (Graphic  :translate-and-euler-angle-trans1orm) 

(x  y  z  azimuth  elevation  roll) 

(let  0 

(setf  H-matrix 

(homogeneous-transform  azimuth  elevation  roll  x  y  z)) 

(setf  transformed-node-list 

(mapcar  #'(lambda  (node-location)  (post-multiply  H-matrix  node-location)) 

node-list)))) 


(defmethod  (graphic  :get-node-polygon-list)  () 

(list  transformed-node-list  polygon-list)) 

(defmethod  (graphic  :initialize)  () 

(setf  node-list  (send  self  :maKe-node-list)) 

(setf  polygon-list  (send  self  :make-polygon-list)) 
(setf  transformed-node-list  node-list) 

(setf  H-matrIx  (unit-matrix  4))) 

(defmethod  (graphic  :get-transformed-node-list)  () 
transformed-node-list) 


•;  CAMERA  FLAVOR  AND  METHODS  TO  USE  GRAPHIC  FLAVOR 

;;  ; Written  by  Dr  Sehung  Kwak 

::  ;for  CS4452 
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THESIS  D.H.  Lewis  18  May  1988 


(defflavor  camera 
(H-matrix 
image-distance 
previous-point 
*camerwindow* 
scale) 

0 

:initabie-instance- variables 

.-gettable-instance-variables 

;outside-accessible-instance-var1ables) 


(defmethod  (camera  :initiaiize) 

0 

(sett  H-rrratrix  (unit-matrix  4)) 

(sett  image-distance  ‘image-distance*) 

(sett  scale  ‘scale*) 

(sett  ‘camerwindow* 

(tv:make-window  'tv;wlndow 
:blinker-p  nil 

.'position  ‘window-upper-lett-comer* 
:inside-width  ‘window-width* 
:inside-height  ‘window-height* 

:name  "VOLUME  WINDOW 
:save-blts  t 
:expose-p  t))) 

(detmethod  (camera  ;inltiali2e-B)  ;  tor  advanced  tunctions 
(window-stats) 

(sett  H-matrix  (unit-matrix  4)) 

(sett  image-distance  ‘image-distance*) 

(sett  scale  ‘scale*) 

(sett  ‘camerwindow* 

(tv:make-window  ’tv.window 
:blinker-p  nil 

:position  (list  (tirst  window-stats) 

(second  window-stats)) 
:inside-width  (third  window-stats) 
'.inside-height  (fourth  window-stats) 
;name  (fifth  window-stats) 

:save-bits  t 
:expose-p  t))) 

(detmethod  (camera  :move) 

(x  y  z  azimuth  elevation  roll) 

(sett  H-matrix 

(homogeneous-transform  azimuth  elevation  roll  x  y  z))) 
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(defmethod  (camera  lake-picture) 

(solid-object) 

(let*  ((node-polygon-iist 

(send  (eval  solid-object)  :get-node-polygon-list)) 
(n^e-vector  (send  seif  :convert-list-of-lists-to-vector 
(first  node-polygon-list))) 

(polygon-list  (second  node-polygon-list))) 

;  (send  *camerwindow*  irefresh)  ;  don’t  need  for  multiple  shots 
(dolist  (polygon  polygon-list) 

(send  self  :draw-polygon  polygon  node-vector)))) 


(defmethod  (camera  :draw-polygon) 

(polygon  node-vector) 

(let  ((first-point  (first  polygon)) 

(rest-points  (cdr  polygon))) 

(send  self  :move-pen  (elt  node-vector  first-point)) 
(dolist  (point  rest-points) 

(send  self  ;draw-line  (elt  node-vector  point))) 
(send  self  :draw-line  (elt  node-vector  first-point)))) 


(defmethod  (camera  .move-pen) 

(point) 

(sett  previous-point  (send  self  ;SCTeen-transform  point))) 


(defmethod  (camera  .draw-line) 

(next-point) 

(let  ((current-point  (send  self  ;screen-transform  next-point))) 
(send  self  :draw-line-on-screen  previous-point  current-point) 
(setf  previous-point  current-point))) 


(defmethod  (camera  ;draw-line-on-screen) 
(from-point  to-point) 

(send  *camerwindow*  :draw-line 

(first  from-point)  (second  from-poInt) 
(first  to-point)  (second  to-point) 
'thickness*)) 


(defmethod  (camera  :convert-list-of-lists-to-vector) 
(list-of-lists) 

(eval  (cons  'vector 

(mapcar  '(lambda  (component-list) 
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(cons  ’list  component-list)) 
list-of-lists)))) 


(defmethod  (camera  :screen-transform) 

(point) 

(let*  ((point-on-camerspace 
(post-multiply 
(matrix-multiply 
H-matrlx 

’((1  0  0  0)  (0  1  0  0)  (0  0  -1  0)  (0  0  0  1)) 

) 

point)) 

(x  (first  point-on-camerspace)) 

(y  (second  point-on-camerspace)) 

(2  (third  point-on-camerspace))) 

(cond  ((equal  0.0  z)  (sett  z  0.00001)) 

(list  (+^(round  (*  scale  (/  (*  image-distance  x)  z)))  (/  *wlndow-wldth*  2)) 

(-  (/  ‘window-height*  2)  (round  (*  scale  (/  (*  image-distance  y)  z))))))) 


(defmethod  (camera  ;kili-camera-window) 

0 

(send  ‘camerwindow*  ;kill)) 

(defun  take-picture  (Camera  List-of-objects) 

(send  (eval  Camera)  :initialize) 

(send  (eval  Camera)  -.move  '2000  '2000  ’2000  '0  ’0.5  ’0.75) 

(loop  for  V  in  List-of-objects 

do  (send  (eval  V)  ;initialize) 

do  (send  (eval  V)  :translate-and-euler-angle-transform  ’-2500 
’-2000  ’-2000  ’0.6  ’0.6  ’-0.1) 
do  (send  (eval  Camera)  itake-picture  V))) 


advanced  camera  functions  D.H.  Lewis 


(defvar  *window-vddth*  700) 

(defvar  ‘window-height*  400) 

(defvar  ‘window-upper-left-corner*  '(10  10)) 

(defvar  ‘scale*  5) 

(defvar ‘image-distance*  120) 

(defvar  ‘thickness*  ’5)  ;  line  thickness,  in  pixels 

(defvar  ‘Ideal*) 

(defvar  ‘low-left-front*) 

(defvar  ‘high-left-front*) 

(defvar  *low-right-front*) 


(defvar  *right-side*) 

(defvar  *left-rear-3/4*) 

(defvar  *top*) 

(defvar  *display-stats*) 

(defvar  *nlkon-1  *) 

(defvar  *nikon-2*) 

(defvar  *nikon-3*) 

(defvar  *nikon-4*) 

(defvar  *nikon-5*) 

(defvar  *nlkor>-6*) 

(defvar  *Hst-of-cameras*) 

(defvar  *window-stats*) 

(defun  make-cameras  () 

(setf  *nikon-1  *  (make-instance  ’camera)) 

(self  *nikon-2*  (make-instance  ’camera)) 

(setf  *nikon-3*  (make-instance  ’camera)) 

(setf  *nikon-4*  (make-instance  ’camera)) 

(setf  *nikon-5*  (make-instance  ’camera)) 

(setf  *list-of-cameras*  ’(*nikon-r  *nikon-2*  *nlkon-3*  *nikon-4*  *nikon-5*)) 

(setf  ‘ideal*  ’(100.0  200.0  4000.0  0.0  0.50  1.3  1.0  1.0  1.0  -1.5  0.3  0.0)) 

(setf  *low-left-front*  ’(100.0  200.0  4000.0  0.0  0.50  1.0  1 .0  1 .0  -1 .5  0.0  0.0  0.0)) 
(setf  *high-left-front*  ’(-100.0  500.0  4000.0  0.0  0.50  1 .0  1 .0  -1.5  0.0  0.0  0.0  0.0)) 
(setf  *low-right-front‘  ’(100.0  100.0  4000.0  0.0  0.5  1 .5  1 .0  1 .0  1 .0  0.0  0.0  0.0)) 
(setf  *right-side*  ’(500.0  0.0  4000.0  0.0  0.0  1.0  1.0  1.0  1.0  0.0  0.0  0.0)) 

(setf  *left-rear-3/4‘  ’(-500.0  0.0  4000.0  0.0  0.0  1 .0  1 .0  1 .0  1 .0  0.0  0.0  0.0)) 

(setf  *top*  ’(0.0  500.0  4000.0  0.0  0.0  0.0  1.0  1.0  1.0  0.0  0.0  0.0)) 

’nil) 


; - main  four  window  display - 

(defun  display  () 

(setf  *window-stats*  ’(’nil 

(10  20  700  400  *air-volumes"  5  140) 

(10  440  200  200  ’top-view;  ground"  5  60) 
(260  440  200  200  "same-view:  ground"  5  60) 
(510  440  200  200  "full-view:  ground"  5  60))) 
(setf  *display-stats*  (list  ’nil 

*high-left-front* 

"top* 

"high-left-front* 

"ideal*)) 

(let  ((air-volumes  ’nil) 

(ground-volumes  ’nil) 

(objects  ’nil)) 

(loop  for  V  in  (universe-volumes  ’universe") 
do  (cond  ((equal  ’ground  (volume-composition  (eval  V))) 

(setf  ground-volumes  (adjoin  V  ground-volumes))) 

(t  (setf  air-volumes  (adjoin  V  air-volumes))))) 

(loop  for  Obs  in  (universe-observers  ‘universe") 


213 


do  (sett  ground-volumes  (adjoin  Obs  ground-volumes)) 
do  (sett  air-volumes  (adjoin  Obs  air-voiumes))) 

(sett  ot^ects  (list  ’nil  air-volumes  ground-volumes  ground-volunies  ground-volumes)) 
(loop  for  N  in  '(1  2  3  4) 
do  (take-picture-4  (nth  N  *list-of-cameras*) 

(nth  N  *window-stats*) 

(nth  N  objects) 

(nth  N  *display-stats*)))) 


’nil) 


; - DISPLAY  VISIBLE  AIR  VOLUMES  (3  WINDOWS) - 

(defun  display-visible  (observer) 

(sett  ‘window-stats*  ’(’nii 

(10  20  700  400  "visible-air- volumes"  5  140) 

’nil 

(260  440  200  200  "same-view-ground"  5  60) 

(510  440  200  200  "full-vlew-ground"  5  60))) 

(sett  ‘dispiay-stats*  (iist  ’nii 

*high-ieft-front* 

'nii 

"high-left-front* 

•ideal* )) 

(let  ((visible-volumes  ’nil) 

(ground-volumes  ’nil) 

(objects  ’nil)) 

(loop  for  V  in  (universe-volumes  'universe*) 
do  (cond  ((equal  'ground  (volume-composition  (eval  V))) 

(sett  ground-volumes  (adjoin  V  ground-volumes)) 

(sett  visible-volumes  (adjoin  V  visible-volumes))) 

((member-p  observer  (volume- visibility  (evai  V))) 

(sett  visible-voiumes  (adjoin  V  visibie-voiumes))))) 

(ioop  for  Obs  in  (universe-observers  "universe") 
do  (sett  ground-volumes  (adjoin  Obs  ground-volumes)) 
do  (sett  visible-volumes  (adjoin  Obs  visible-volumes))) 

(sett  objects  (list  'nil  visible-volumes  'nil  ground-volumes  ground-volumes)) 
(loop  for  N  in  '(1  3  4) 

do  (take-picture-4  (nth  N  "list-of-cameras") 

(nth  N  "window-stats") 

(nth  N  objects) 

(nth  N  "display-stats")))) 


; - DISPLAY  NON  VISIBLE  AIR  VOLUMES  (3  WINDOWS) - 

(defun  display-not-visible  (observer) 

(sett  "window-stats*  ’(’nil 

(10  20  700  400  "non-visible-air-volumes"  5  140) 
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’nil 

(260  440  200  200  "same-view-ground"  5  60) 

(510  440  200  200  "full- view-ground"  5  60))) 

(self  "display-stats*  (list  ’nil 

"high-left-front* 

’nil 

"high-left-front* 

"Weal* )) 

(let  ((invisible-volumes  ’nil) 

(ground-volumes  ’nil) 

(objects  ’nil)) 

(loop  for  V  in  (universe-volumes  "universe") 
do  (cond  ((equal  ’ground  (volume-composition  (eval  V))) 

(setf  ground-volumes  (adjoin  V  ground-volumes)) 

(setf  invisible-volumes  (adjoin  V  invisible-volumes))) 

((not  (member-p  observer  (volume-visibility  (eval  V)))) 

(setf  invisible-volumes  (adjoin  V  invisible-volumes))))) 

(loop  for  Obs  In  (universe-observers  "universe") 
do  (setf  ground-volumes  (adjoin  Obs  ground-volumes)) 
do  (setf  invisible-volumes  (adjoin  Obs  invisible-volumes))) 

(setf  objects  (list  ’nil  invisible-volumes  ’nil  ground-volumes  ground-volumes)) 
(loop  for  N  in  ’(1  3  4) 

do  (take-picture-4  (nth  N  "list-of-cameras") 

(nth  N  "window-stats") 

(nth  N  objects) 

(nth  N  "display-stats")))) 

’nil) 


DISPLAY  SELECTED  VOLUMES  AND  THE  GROUND  (2  WINDOWS) 


(defun  display-volumes  (llst-of-volumes) 

(setf  "window-stats*  ’(’nil 

(10  20  700  400  "desired- volumes"  5  140) 
’nil 

(510  440  200  200  "full-view-ground"  5  60) 
’nil)) 

(setf  "display-stats*  (list  ’nil 

"high-left-front* 

’nil 

"high-left-front* 

'nil)) 

(let  ((objects  ’nil) 

(ground-volumes  ’nil)) 

(loop  for  V  in  (universe-volumes  "universe") 
do  (corrd  ((equal  ’ground  (volume-composition  (eval  V))) 

(setf  ground-volumes  (adjoin  V  ground-volumes))))) 
(loop  for  Obs  in  (universe-observers  "universe") 
do  (setf  ground-volumes  (adjoin  Obs  ground-volumes))) 

(setf  objects  (list  ’nil 
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list-of-volumes 

’nil 

ground-volumes 

’nil)) 

(loop  for  N  in  ’(1  3) 

do  (take-picture-4  (nth  N  *list-of-cameras*) 

(nth  N  *wlndow-stats*) 
(nth  N  objects) 

(nth  N  *display-stats*)))) 


; - DISPLAY  PATH  AND  GROUND  (3  WINDOWS) - 

(defun  display-path  (path-name) 

(setf  *window-stats*  ’(’nil 

(10  20  600  380  "Patti-over-ground"  5  140) 

(10  420  600  290  ’’Alternate-view  ”  5  140) 

(618  200  200  200  ’Top-view'  5  60) 

(618  420  200  200  'Low-side  view'  5  60))) 

(setf  'display-stats*  (list  ’nil 

'ideal* 

'high-left-front* 

'top* 

'right-side')) 

(let  ((objects  ’nil) 

(ground-volumes  ’nil)) 

(loop  for  V  In  (universe-volumes  'universe') 
do  (cond  ((equal  ’ground  (volume-composition  (eval  V))) 

(setf  ground-volumes  (adjoin  V  ground-volumes))))) 

(setf  ground-volumes  (append  (universe-observers  'universe')  ground-volumes)) 
(setf  objects  (list  ’nil 

(adjoin  path-name  ground-volumes) 

(adjoin  path-name  ground-volumes) 

(adjoin  path-name  ground-volumes) 

(adjoin  path-name  ground-volumes))) 

(loop  for  N  in  ’(1  2  3  4) 
do  (take-plcture-4  (nth  N  *list-of-cameras*) 

(nth  N  'window-stats') 

(nth  N  objects) 

(nth  N  'display-stats')))) 

’nil) 


(defun  display-paths  (list-of-paths) 

(setf  'window-stats*  ’(’nil 

(10  20  600  380  'Paths-over-ground'  5  140) 
(10  420  600  290  'Aftemate-vlew '  5  140) 
(618  200  200  200  Top-view'  5  60) 

(618  420  200  200  'Low-side  view'  5  60))) 
(setf  'dispiay-stats*  (fist  ’nil 
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‘ideal* 

*hlgh-teft-front* 

•top* 

*right-skle*)) 

(let  ((objects  ’nil) 

(ground-volumes  ’nil)) 

(loop  for  V  in  (universe-volumes  ‘universe*) 
do  (cond  ((equal  'ground  (volume-composition  (eval  V))) 

(sett  ground-volumes  (adjoin  V  ground-volumes))))) 

(sett  ground-volumes  (append  (universe-observers  ‘universe*)  ground-volumes)) 
(sett  ot^ects  (list  'nil 

(append  list-of-paths  ground-volumes) 

(append  list-of-paths  ground-volumes) 

(append  list-of-paths  ground-volumes) 

(append  list-of-paths  ground-volumes))) 

(loop  for  N  in  '(1  2  3  4) 
do  (take-picture-4  (nth  N  *list-of-cameras*) 

(nth  N  ‘window-stats*) 

(nth  N  objects) 

(nth  N  ‘display-stats*)))) 

’nil) 


; - SIMPLE  CAMERA  ORDERS  FOR  A  PICTURE  (MANUAL  CONTROL) - 

(defun  take-picture-3  (List-of-objects  x  y  z  az  roll  rot  ox  oy  oz  oaz  oroll  orot) 

(let  ((Camera  ’*nikon*)) 

(send  (eval  Camera)  :initialize) 

(send  (eval  Camera)  :move  (-  x)  (-  y)  z  az  roll  rot ) 

(loop  for  V  in  List-of-objects 

do  (send  (eval  V)  :initialize) 

do  (send  (eval  V)  ;translate-and-euler-angle-transform  ox  oy  oz  oaz  oroll  orot) 
do  (send  (eval  Camera)  :take-pjcture  V)))) 


-ADVANCED  CAMERA  ORDERS  FOR  A  PICTURE  (SEMI-AUTOMATIC  CONTROL)- 


(defun  take-picture-4  (Camera  Window-stats  LIst-of-objects  view-stafs) 
(cond  ((or  (null  view-stats) 

(null  list-of-objects)) 

(returrvfrom  take-picture-4  ’nil))) 

(setf  *wlndow-wldth*  (third  window-stats)) 

(setf  ‘window-height*  (fourth  window-stats)) 

(setf  ‘scale*  (sixth  window-stats)) 

(setf  ‘Image-distance*  (seventh  window-stats)) 

(send  (eval  Camera)  ;initialize-B  Window-stats) 

(send  (eval  Camera)  .move  (-  (first  view-stats))  ;  x 
(- (second  view-stats))  ;y 

(third  view-stats)  ;  z 
(fourth  view-stats)  ;  azimuth 
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(fifth  view-stats)  ;  roli 
(sixth  view-stats))  ;  rotation 


(loop  for  V  in  LIst-of-objects 

do  (send  (eval  V)  .-initialize) 

do  (send  (eval  V)  ;translate-and-euler-angle-transform  (nth  6  view-stats) 

(nth  7  view-stats) 

(nth  8  view-stats) 
(nth  9  view-stats) 
(nth  10  view-stats) 
(nth  1 1  view-stats)) 


do  (send  (eval  Camera)  :take-picture  V) 

do  (let  ((object-type  (string-trim  ’"10123456789  ’  V))) 

(cond  ((string-equal  object-type  ’"observer") 

(let*  ((obs-point  (first  (send  (eval  V)  :get-transformed-node-list))) 
(screen-point  (send  (evsd  Camera)  iscreen-transform  obs-point))) 
(send  (eval  (camera-*camerwindow*  (eval  Camera))) 

:set-cursorpos  (-  (first  screen-point)  ’30) 

(-  (second  screen-point)  ’5)) 

(send  (eval  (camera-*camerwindow*  (eval  Camera))) 
:display-lozenged-string  ’"obs"))) 

((string-equal  object-type  ’"patti") 

(let*  ((start-point  (first  (send  (eval  V)  :get-transformed-node-list))) 
(end-point 

(first  (last  (send  (eval  V)  :get-transformed-node-list)))) 
(SCTeen-start-point 

(send  (eval  Camera)  rscreen-transfomn  start-point)) 
(screen-end-point 

(send  (eval  Camera)  tscreen-transform  end-point))) 

(cond  ((<  ’50000  (*  *window-width*  *wlndow-height*)) 

(send  (eval  (camera-*camerwlndow*  (eval  Camera))) 
:set-cursorpos  (-  (first  screen-start-point)  ’43) 

(-  (second  saeen-start-point)  ’5)) 

(send  (eval  (camera-*camerwindow*  (eval  Camera))) 
:display-lozenged-str1ng  ’"start") 

(send  (eval  (camera-*camerwindow*  (eval  Camera))) 
:set-cursorpos  (+  (first  screen-end-point)  ’3) 

(-  (second  screen-end-point)  ’5)) 

(send  (eval  (camera-*camerwindow*  (eval  Camera))) 
:display-lozenged-string  ’"end"))))))))) 
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FILE  NAME:  Kinematics . lisp 


rotation  and  transisrtion  code  cs4452  17may88 


(defun  transpose  (A) 

(cond  ((null  (cdr  A))  (mapcar  ’list  (car  A))) 

(t  (mapcar  'cons  (car  A)  (transpose  (cdr  A)))))) 

(defun  dot-product  (x  y)  ;A  vector  is  a  list  of  numerical  atoms. 

(apply  '•*-  (mapcar  '*  x  y)))  ;A  matrix  is  a  list  of  vectors  representing 

(defun  cross-product  (x  y) 

(list  (-  (*  (cadr  x)  (caddr  y))  (*  (caddr  x)  (cadr  y))) 

(-  (•  (caddr  x)  (car  y))  (*  (car  x)  (caddr  y))) 

(-  (*  (car  X)  (cadr  y))  (*  (cadr  x)  (car  y))))) 

(defun  post-multiply  (M  x)  ;the  rows  of  the  matrix. 

(cond  ((null  (cdr  M))  (list  (dot-product  (car  M)  x))) 

(t  (cons  (dot-product  (car  M)  x)  (post-multiply  (cdr  M)  x))))) 

(defun  pre-multiply  (x  M) 

(post-rTHJitiply  (transpose  M)  x)) 

(defun  matrix-muHiply  (A  B) 

(cond  ((null  (cdr  A))  (list  (pre-multiply  (car  A)  B))) 

(t  (cons  (pre-multiply  (car  A)  B)  (matrix-multiply  (cdr  A)  B))))) 

(defun  cycle-left  (L)  (mapcar  ’row-cycle-left  L)) 

(defun  row-cycle-left  (R)  (append  (cdr  R)  (list  (car  R)))) 

(defun  cyde-up  (M)  (append  (cdr  M)  (list  (car  M)))) 

(defun  unit-vector  (one-column  length) 

(do  ((n  length  (1-n)) 

(R  nil  (cons  (cond  ((=  one-column  n)  1)  (t  0))  R))) 

((zerop  n)  R))) 

(defun  concat-matrix  (A  B) 

(corid  ((null  A)  B) 

(t  (cons  (append  (car  A)  (car  B))  (concat-matrix  (cdr  A)  (cdr  B)))))) 
(defun  augment  (A)  (concat-matrix  A  (unit-matrix  (length  A)))) 

(defun  normalize-row  (R)  (scalar-multiply  (/ 1.0  (car  R))  R)) 

(defun  scalar-multiply  (a  x) 

(cond  ((null  x)  nil) 

(t  (cons  (*  a  (car  x))  (scalar-multiply  a  (cdr  x)))))) 

(defun  solve-first-column  (M) 

(do*  ((LI  M  (cdr  LI)) 

(L2  (normalize-row  (car  M))) 

(L3  (list  L2)  (cons  (vector-add  (car  LI) 

(scalar-multiply  (-  (caar  LI))  L2))  L3))) 

((null  (cdr  LI))  (reverse  L3)))) 

(defun  vector-add  (x  y)  (mapcar  ’+  x  y)) 

(defun  first-n  (n  R) 

(cond  ((zerop  n)  nil) 

(t  (cons  (car R)  (first-n  (1-n)  (cdr  R)))))) 

(defun  square-car  (M) 

(do  ((m  (length  M)) 
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(LI  M(cdrLI)) 

(L2  nil  (cons  (first-n  m  (car  LI))  L2))) 

((null  LI)  (reverse  L2)))) 

(setqA’((1  1  1)(21  2)  (3  2  3))) 

(setq  B  ’((1  1  2)  (1  2  3)  (2  3  1))) 

(defun  ncdr  (n  L)  (cond  ((zerop  n)  L)  (t  (cdr  (ncdr  (1-  n)  L))))) 

(defun  near  (n  L)  (cond  ((zerop  n)  nil) 

(t  (cons  (car  L)  (near  (1-  n)  (cdr  L)))))) 

(defun  nmax-car-first  (n  L) 

(append  (max-car-first  (near  n  L))  (ncdr  n  L))) 

(defun  matrix-inverse  (M) 

(do  ((Ml  (max-car-first  (augment  M)) 

(cond  ((null  Ml)  nil) 

(t  (nmax-car-first  n  (cycle-left  (cyde-up  Mf )))))) 

(n  (1-  (length  M))  (1-  n))) 

((or  (minusp  n)  (null  Ml))  (cond  ((null  Ml)  nil)  (t  (square-car  Ml)))) 
(setq  Ml  (cond  ((zerop  (caar  Ml))  nil)  (t  (solve-first-column  M1 )))))) 
(defun  max-car-first  (L) 

(cond  ((null  (cdr  L))  L) 

(t  (If  (>  (abs  (caar  L))  (abs  (caar  (max-car-first  (cdr  L)))))  L 
(append  (max-car-first  (cdr  L))  (list  (car  L))))))) 

(defun  dh-matrix  (cosrotate  sinrotate  costwist  sintwist  length  translate) 
(list  (list  cosrotate  (•  (*  costwist  sinrotate)) 

(*  sintwist  sinrotate)  (*  length  cosrotate)) 

(list  sinrotate  (*  costwist  cosrotate) 

(-  (*  sintwist  cosrotate))  (‘  length  sinrotate)) 

(list  0.  sintwist  costwist  translate)  (list  0.  0. 0.  f .))) 


(defun  homogeneous-transform  (azimuth  elevation  roll  x  y  z)  r 

(rotation-and-translation  (sin  azimuth)  (cos  azinuith)  (sin  elevation) 

(cos  elevation)  (sin  roll)  (cos  roll)  x  y  z)) 


(defun  rotation-and-translation  (spsi  cpsi  sth  c^  sphi  ephi  x  y  z) 
(list  (list  (*  cpsi  cth)  (-  (*  cpsi  sth  sphi)  (*  spsi  ephi)) 

(+  (*  cpsi  sth  ephi)  (*  spsi  sphi))  x) 

(list  (*  spsi  cth)  (+  (*  cpsi  ephi)  (*  spsi  sth  sphi)) 

(-  (*  spsi  sth  ephi)  (*  cpsi  sphi))  y) 

(list  (-  sth)  (*  cth  sphi)  (*  cth  ephi)  z) 

(listO.  0. 0.  t.))) 

(defun  A01  (d1) 

(dh-matrix  01  01  0  d1)) 

(defun  A12(d2) 

(dh-matrIx  0  1010  d2)) 

(defun  A23  (d3) 

(dh-matrix  0  10  10  d3)) 

(defmacro  A03  (d1  d2  d3) 

•(chain-multiply  '((AOI  ,d1)  (A12  ,d2)  (A23  ,d3)))) 
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(defun  A34  (theta4) 

(dh-matrix  (cos  theta4)  (sin  theta4)  0  10  0)) 

(defun  A45  (thetaS) 

(dh-matrix  (cos  thetaS)  (sin  thetaS)  010  0)) 

(defun  A56  (thetaS) 

5  (dh-matrix  (cos  thetaS)  (sin  thetaS)  010  0)) 

(defmacro  A3S  (theta4  thetaS  thetaS) 

'(chain-multiply  ‘((A34  ,theta4)  (A45 , thetaS)  (ASS  .ttietaS)))) 

(defun  AOS  (d  1  d2  d3  theta4  thetaS  ^etaS) 

(matrix-multiply  (AOS  d1  d2  d3)  (ASS  theta4  thetaS  thetaS))) 

(setq  ASbody  ’((0  0  1  0)  (1  0  0  0)  (0  1  0  0)  (0  0  0  1))) 

(defun  homogeneous-transforml  (azimuth  elevation  roll  x  y  z) 

(matrix-multiply  (AOS  z  x  y  (+  azimuth  pi)  (-  elevation  (/  pi  2)) 

(+  roll  pi))  ASbody)) 

(setq  BSbody  ’((1  0  0  0)  (0  0  -1  0)  (0  1  0  0)  (0  0  0  1))) 

(defun  homogeneous-transform2  (azimuth  elevation  roll  x  y  z) 

(matrix-multiply  (AOS  z  x  y  azimuth  elevation  roll )  BSbody )) 

;  changes;  D.H.Lewls  17  May  88 

(defun  unit-rr)atrix  (L) 

(loop  for  i  from  L  downto  1 

collect  (loop  for  j  from  L  downto  1 
when  (equal  i  j) 
collect  1 
else  collect  0 
finally) 

'  finally)) 

(defun  chain-multiply  (L) 

>  (cond  ((equal  (length  L)  2)  (matrix-multiply  (eval  (first  L))  (eval  (second  L)))) 

(t  (setq  temp  (matrix-multiply  (eval  (first  L))  (eval  (second 
L))))  (chain-multiply  (push  'temp  (cddr  L)))))) 
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